Drawing Robot Source Code

/* Shaun Bowman
    Feb 5 2011
    
    Path Finding Robot POC
    Rev: add r_cd, add non-unit supports
    
    A:    
*/
#include   
Servo myservoBase;  // create servo object to control a servo 
Servo myservoFloat;

int MYBASE_PIN = 8;
int MYFLOAT_PIN = 12;

float   x_i, y_i;
float   x_Prev, y_Prev, x_Curr, y_Curr;
float   b_Ang, a_Ang, a_Ang_Sum, b_Ang_Sum;
float   a_Sqr_temp, b_Sqr_temp, a_temp, b_temp, delta_x, delta_y, mul_x, mul_y;
float   x_Loop_Best, y_Loop_Best;
float   a_disc, b_disc, c_disc, d_disc, a_inv, b_inv, c_inv, d_inv, discriminate;
int     inByte = 0;

// main physical setup
int    BASE_INIANG = 0;
int    FLOAT_INIANG = 90;
float  CloseEnough   = 0.010;
float  StepSize      = 0.005; // must be smaller than Close Enough
float  rbc = 1.0;        // 1;        // 5.090"
float  rb = 0.9737;    // 0.9737;   // 4.956" 
float  rcd = 0.114;     // 0.114;    // 0.582" perpendicular to bc at point c, distance of rcd
float  x_1   = rb + rcd;
float  y_1   = rbc;
float  x_2   = x_1;
float  y_2   = x_1; 
float  normalize = sqrt(rb*rb + rbc*rbc + rcd*rcd); // normalization factor for jacobian; allows for non-unit lengths
float  n_jacIters = 8; // number of jacobian iterations before command
float  diff_one, diff_two, diff_three;


int BASE_LOWLIM = 30 + 3;
int BASE_HIGHLIM = 150 - 3;
int BASE_0DEGOFFSET = 70;
int BASE_ROTATION = 1;
int BASE_MAXSTEP = 10;
int BASE_BACKLASH = 0;
int FLOAT_LOWLIM = 46 + 3;
int FLOAT_HIGHLIM = 143 - 3;
int FLOAT_90DEGOFFSET = 4;
int FLOAT_BACKLASH = 3;
int FLOAT_ROTATION = -1;
int FLOAT_MAXSTEP = 10; // max step size for float
int DELAY_CMND = 10;
int VERBOSE = 0; // display verbage

int deg_Cmnd_B_Prev = FLOAT_INIANG;
int deg_Cmnd_B_Curr = FLOAT_INIANG;
int deg_Cmnd_B_int = FLOAT_INIANG;
int deg_Cmnd_A_Prev = BASE_INIANG;
int deg_Cmnd_A_Curr = BASE_INIANG;
int deg_Cmnd_A_int = BASE_INIANG;

void setup(){
  Serial.begin(9600);  // start serial port at 9600 bps:

  deg_Cmnd_B_Prev = FLOAT_INIANG + FLOAT_90DEGOFFSET; // real servo units
  deg_Cmnd_B_Curr = FLOAT_INIANG + FLOAT_90DEGOFFSET; // real servo units
  deg_Cmnd_B_int = FLOAT_INIANG + FLOAT_90DEGOFFSET; // real servo units
  deg_Cmnd_A_Prev = BASE_INIANG + BASE_0DEGOFFSET; // real servo units
  deg_Cmnd_A_Curr = BASE_INIANG + BASE_0DEGOFFSET; // real servo units
  deg_Cmnd_A_int = BASE_INIANG + BASE_0DEGOFFSET; // real servo units
  digitalWrite(MYBASE_PIN,LOW);
  digitalWrite(MYFLOAT_PIN,LOW);
  a_Ang_Sum = (float)BASE_INIANG*3.14159/180;
  b_Ang_Sum = (float)FLOAT_INIANG*3.14159/180; 
}

void loop(){
 initialize_A_and_B();
 while( inByte != 1){
   inByte = Type1ToStart();
   delay(100);
 }
  
 x_Curr  =  x_1;
 y_Curr  =  y_1;
 x_Prev  =  x_1;
 y_Prev  =  y_1;
 x_i  = x_1;
 y_i  = y_1;
 x_Loop_Best = x_i;
 y_Loop_Best = y_i;
 delay(2000);
  
  Serial.println("Current X Y Position:");
  PrintCoord(x_1, y_1);
  
  // MAIN LOOP
  while(true){
    Serial.println("Target X co-ord (x.xxx): ");          // start get target X  
    x_Loop_Best = x_i;
    x_Curr = x_i;
    x_1 = x_2;
    x_2 = getCoord();
    Serial.println("Target Y co-ord (y.yyy): ");          // start get target Y
    y_Loop_Best = y_i;
    y_Curr = y_i;
    y_1 = y_2;
    y_2 = getCoord();  
    Serial.println("Final Target: X_2, Y_2:");
    PrintCoord(x_2,y_2);  // ok    
    Serial.println("");
                    
    while( ! ( ( abs(x_2 - x_Curr) + abs(y_2 - y_Curr)) < CloseEnough )){
      delta_x = x_2 - x_Curr;
      delta_y = y_2 - y_Curr;   
      x_Prev = x_i;
      y_Prev = y_i;
      mul_x = 1;
      mul_y = 1;

     // COMPUTE SLOPE TO MOVE ALONG, RESPECTING MAX STEP 
     if( (abs(delta_x) > abs(delta_y)) && (delta_x != 0)){
       mul_y = abs(delta_y)/abs(delta_x);
     }
     if( (abs(delta_y) >= abs(delta_x) ) && (delta_y != 0)){
       mul_x = abs(delta_x)/abs(delta_y);
     }
     if( abs(delta_x) < StepSize ){
       mul_x = 0;
     }
     if( abs(delta_y) < StepSize ){
       mul_y = 0;
     }
     if( delta_y != 0){
       y_i = delta_y/abs(delta_y)*StepSize*mul_y + y_Prev;                   // Note div by zero bug y_2 = y_1
     }
     if( delta_x != 0){
       x_i = delta_x/abs(delta_x)*StepSize*mul_x + x_Prev;                    // Note div by zero bug     
        if(VERBOSE >= 3){
         Serial.println("x_i:");
         Serial.print(x_i,4);
         Serial.println("y_i:");
         Serial.print(y_i,4);
         Serial.println("delta_y:");
         Serial.print(delta_y,4);
         Serial.println("delta_x:");
         Serial.print(delta_x,4);
         Serial.println("mul_y:");
         Serial.print(mul_y,4);
         Serial.println("mul_x:");
         Serial.print(mul_x,4);       
        }
     }
      x_Curr = x_i;
      y_Curr = y_i;

  // CALCULATE COMMAND ANGLES - INVERSION METHOD:
  // float a_disc, b_disc, c_disc, d_disc, a_inv, b_inv, c_inv, d_inv, discriminate;
  // drc/dx = x_i - x_Prev    drc/dy = y_i - y_Prev      rb = 1.0 rbc = 1.0, ao = a_Curr, bo = b_Curr      
      for( int i_iter = 1; i_iter <= n_jacIters; i_iter ++){
        a_disc = (-rb*sin(a_Ang_Sum) - rbc*sin(a_Ang_Sum + b_Ang_Sum)             + rcd*sin(3.141592/2-a_Ang_Sum-b_Ang_Sum))/normalize;
        b_disc = (-rbc*sin(a_Ang_Sum + b_Ang_Sum)                                 - rcd*cos(3.141592/2-a_Ang_Sum-b_Ang_Sum))/normalize;
        c_disc = (rb*cos(a_Ang_Sum) + rbc*cos(a_Ang_Sum + b_Ang_Sum)              + rcd*sin(3.141592/2-a_Ang_Sum-b_Ang_Sum))/normalize;
        d_disc = (rbc*cos(a_Ang_Sum + b_Ang_Sum)                                  - rcd*cos(3.141592/2-a_Ang_Sum-b_Ang_Sum))/normalize;        
        if( a_disc*d_disc - b_disc*c_disc != 0){
          discriminate = 1/(a_disc*d_disc - b_disc*c_disc);
          a_inv = d_disc*discriminate;
          b_inv = -b_disc*discriminate;
          c_inv = -c_disc*discriminate;
          d_inv = a_disc*discriminate;
          a_Ang = (a_inv*(x_i - x_Loop_Best) + b_inv*(y_i - y_Loop_Best));  // note: using x&y_Loop_Best here limits accruing error when moving multiple 

steps
          b_Ang = (c_inv*(x_i - x_Loop_Best) + d_inv*(y_i - y_Loop_Best));  // note: using x&y_Loop_Best here limits accruing error when moving multiple 

steps
          a_Ang_Sum = a_Ang_Sum + a_Ang;
          b_Ang_Sum = b_Ang_Sum + b_Ang;
          x_Loop_Best = rb*cos(a_Ang_Sum) + rbc*cos(a_Ang_Sum + b_Ang_Sum)         + rcd*cos(3.141592/2-a_Ang_Sum-b_Ang_Sum);
          y_Loop_Best = rb*sin(a_Ang_Sum) + rbc*sin(a_Ang_Sum + b_Ang_Sum)         + rcd*sin(3.141592/2-a_Ang_Sum-b_Ang_Sum);
          if(VERBOSE >= 2){
            Serial.println("Iteration - Inversion method - x100: (a, b)");
            PrintCoord(a_Ang*100, b_Ang*100);          
          }
        }
        else{
          Serial.println("Epic discriminate div by zero fail");    
        }
      }
      if(VERBOSE >= 2){
        Serial.println(" ");
        Serial.println(" ");          
        Serial.println("Cmnd Angles - Inversion method - deg: (a, b)");
        PrintCoord(a_Ang_Sum*180/3.14159, b_Ang_Sum*180/3.14159);
        Serial.println("Commanded Position - Inversion method: (x,y)");
        PrintCoord(x_Loop_Best, y_Loop_Best);      
      }
      Cmnd_A_and_B(a_Ang_Sum*180/3.14159, b_Ang_Sum*180/3.14159);
      
   }
  Serial.println("Final Target: X_2, Y_1:");
  PrintCoord(x_2,y_2);  // ok    
  Serial.println("Final Dest: actX_2, actY_1:");
  PrintCoord(x_Loop_Best,y_Loop_Best);  // ok    
  }  // end while(true) loop;
  
 Serial.println("All Done !");

} // end main loop();



// -----------------SUBROUTINES--------------------------------------------------
// Get Co-ordinate function; reads in x.xxx via serial, uses charToFloat to return a float
float getCoord(){
  #define NDEC_getCoord    3  // x.xxx = 3 decimals
  #define NCHAR_getCoord   5  // x.xxx = 5 characters
  #define INTERMINATOR_getCoord   13 // x.xxx = 4 characters
  char  a_inRaw_Char[NCHAR_getCoord];
  int   inCount_getCoord = 0;
  
  float a_inRaw;  
  int coord_inTemp = 0;
  while(inCount_getCoord < NCHAR_getCoord){
    if(Serial.available() > 0){
      coord_inTemp = Serial.read();
      if ((coord_inTemp == INTERMINATOR_getCoord)){
        break;
      }
      a_inRaw_Char[inCount_getCoord] = coord_inTemp;
      inCount_getCoord ++;
    }
    refresh_A_and_B(); //refresh servo position while waiting
  }
  inCount_getCoord = 0;
  
  a_inRaw = charToFloat(a_inRaw_Char, NDEC_getCoord);
  return a_inRaw;
} 
// Get Co-ordinate function; reads in x.xxx via serial, uses charToFloat to return a float
float charToFloat(char charTemp[], int n_Dec){
  int worked = 1;
  float a_charToFloat;
  float fl_temp;
  if ( ((float)(charTemp[0] - 48) > 9.0001) || ((float)(charTemp[0] - 48) < -0.0001) ){
    worked = 0;
  }
  a_charToFloat = (float)(charTemp[0] - 48);
  for (int i_char = 1; i_char <= n_Dec; i_char ++){
    a_charToFloat = a_charToFloat + (float)(charTemp[i_char + 1] - 48)/((float)pow(10,i_char));
    if ( ((float)(charTemp[i_char + 1] - 48) > 9.0001) || ((float)(charTemp[i_char + 1] - 48) < -0.0001) ){
      worked = 0;
    }
  }
  if ( worked != 1){
    Serial.println("Failed parse");
    return 1.0;
  }
  else{
    return a_charToFloat;
  }
}

void Cmnd_A_and_B(float deg_Cmnd_A, float deg_Cmnd_B){
  deg_Cmnd_A_int = ((int)deg_Cmnd_A - BASE_INIANG)*BASE_ROTATION + BASE_INIANG + BASE_0DEGOFFSET;
  if(VERBOSE >=2){
    Serial.println("Base Cmnd int:");
    Serial.println(deg_Cmnd_A_int); 
  }
  deg_Cmnd_A_Curr = myservoBase.read();
  
//  if( (deg_Cmnd_A_Curr - deg_Cmnd_A_Prev) > 0 && (deg_Cmnd_A_int - deg_Cmnd_A_Curr) < 0){ // was inc, now dec, need backlash
//      deg_Cmnd_A_int = deg_Cmnd_A_int - BASE_BACKLASH;
//         Serial.println("Base Cmnd neg backlash int:");
//        Serial.println(deg_Cmnd_A_int);       
//         Serial.println("Base Cmnd A Curr:");
//        Serial.println(deg_Cmnd_A_Curr);
//         Serial.println("Base Cmnd A Prev:");
//        Serial.println(deg_Cmnd_A_Prev);    
//  }
//  if( (deg_Cmnd_A_Curr - deg_Cmnd_A_Prev) < 0 && (deg_Cmnd_A_int - deg_Cmnd_A_Curr) > 0){ // was dec, now inc, need backlash
//      deg_Cmnd_A_int = deg_Cmnd_A_int + BASE_BACKLASH;
//         Serial.println("Base Cmnd pos backlash int:");
//        Serial.println(deg_Cmnd_A_int);         
//  }  
  
// b:
  deg_Cmnd_B_int = ((int)deg_Cmnd_B - FLOAT_INIANG)*FLOAT_ROTATION + FLOAT_INIANG + FLOAT_90DEGOFFSET;
  if(VERBOSE >=2){
    Serial.println("Float Cmnd int:");
    Serial.println(deg_Cmnd_B_int);  
  }
  deg_Cmnd_B_Curr = myservoFloat.read();
  
//  if( (deg_Cmnd_B_Curr - deg_Cmnd_B_Prev) > 0 && (deg_Cmnd_B_int - deg_Cmnd_B_Curr) < 0){ // was inc, now dec, need backlash
//      deg_Cmnd_B_int = deg_Cmnd_B_int - FLOAT_BACKLASH;
//         Serial.println("Float Cmnd neg backlash int:");
//        Serial.println(deg_Cmnd_B_int);
//        Serial.println("Deg B Prevs:");
//        Serial.println(deg_Cmnd_B_Prev); 
//        Serial.println("deg_Cmnd_B_Curr:");
//        Serial.println(deg_Cmnd_B_Curr);        
//  }
//  if( (deg_Cmnd_B_Curr - deg_Cmnd_B_Prev) < 0 && (deg_Cmnd_B_int - deg_Cmnd_B_Curr) > 0){ // was dec, now inc, need backlash
//      deg_Cmnd_B_int = deg_Cmnd_B_int + FLOAT_BACKLASH;
//        Serial.println("Float Cmnd pos backlash int:");
//        Serial.println(deg_Cmnd_B_int);          
//        Serial.println("Deg B Prevs:");
//        Serial.println(deg_Cmnd_B_Prev); 
//        Serial.println("deg_Cmnd_B_Curr:");
//        Serial.println(deg_Cmnd_B_Curr);         
//  }  

// SEND COMMAND ANGLES TO SERVOS:
  if( (deg_Cmnd_A_int < BASE_HIGHLIM) && (deg_Cmnd_A_int > BASE_LOWLIM) && (abs(deg_Cmnd_A_int - deg_Cmnd_A_Curr) < BASE_MAXSTEP)){ // limit test
      deg_Cmnd_A_Prev = deg_Cmnd_A_Curr;
      myservoBase.write(deg_Cmnd_A_int);
   if(VERBOSE >= 2){
      Serial.println("Base Cmnd:");
      Serial.println(deg_Cmnd_A_int);      
   }
  }
  
  if( (deg_Cmnd_B_int < FLOAT_HIGHLIM) && (deg_Cmnd_B_int > FLOAT_LOWLIM) && (abs(deg_Cmnd_B_int - deg_Cmnd_B_Curr) < FLOAT_MAXSTEP)){ // limit test
      deg_Cmnd_B_Prev = deg_Cmnd_B_Curr;
      myservoFloat.write(deg_Cmnd_B_int);
      if(VERBOSE >= 2){
        Serial.println("Float Cmnd:");
        Serial.println(deg_Cmnd_B_int);
      }
  }

  delay(DELAY_CMND);
}


void initialize_A_and_B(){
  myservoBase.attach(MYBASE_PIN);  // attaches the servo on pin 9 to the servo object 
  myservoFloat.attach(MYFLOAT_PIN);  // attaches the servo on pin 9 to the servo object   
  myservoBase.write(BASE_INIANG + BASE_0DEGOFFSET);
  myservoFloat.write(FLOAT_INIANG + FLOAT_90DEGOFFSET); 
  Serial.println("Base Ini:");
  Serial.println(BASE_INIANG);
  Serial.println("Float Ini:");
  Serial.println(FLOAT_INIANG);  
  Serial.println("To begin: 1 (enter) Cmnd Destination: x.xxx (enter) y.yyy (enter)"); 
}

void refresh_A_and_B(){
  myservoBase.write(myservoBase.read());
  myservoFloat.write(myservoFloat.read()); 
}

void PrintCoordAndAng(float a_Coord, float b_Coord, float x_Coord, float y_Coord){
  Serial.print("a_i: ");
  Serial.print(a_Coord,4);
  Serial.print(" b_i: ");
  Serial.print(b_Coord,4);
  Serial.print(" x_i: ");
  Serial.print(x_Coord,4);
  Serial.print(" y_i: ");
  Serial.print(y_Coord,4);  
  Serial.println();
}

void PrintCoord(float x_i_PrintCoord, float y_i_PrintCoord){
  Serial.print("x_i: ");
  Serial.print(x_i_PrintCoord);
  Serial.print("y_i: ");
  Serial.print(y_i_PrintCoord);
  Serial.println();
}


int Type1ToStart(){
  if (Serial.available() > 0) {
    inByte = Serial.read();        // get incoming byte:
    return ((int)inByte - 48);
  }
  else{
    return 0;
  }  
}


Leave a Reply

Your email address will not be published. Required fields are marked *