For-Loop Questions and a Complex Biped

Okay, I don’t know how much help i’ll get, but here’s hoping…

I have a sketch, and it involves a nested for loop command. Now the trouble is, this is running on my biped, and i cant seem to get it to run right.

Basically this is how it is set up…
For(Leg = 1 ----> Leg = 2){
For(Start Leg Location ------> End Leg Location){
CODE
}

I cant really explain it, i need the first leg to move, then the second AND as the second one is moving bring the first leg back to Start…
Heres part of the code…

for(leg = 1; leg <= 2; leg++){
    for(x = xmin; x <= xmax; x = x + xstep){
        y = yCenter + sqrt(sq(CirRadius)-sq(x-xCenter));
        fracdone = (x - xmin) / (xmax-xmin);
        
        CheckHypotenuse();
           if(CheckHypotenuse()==0){
             return;
           }
         ik2();
         normalize();
         
         if(leg == 1){ 

           servo3pulse = D2Pminus(a1500pulse, theta1);
           servo4pulse = D2Pplus(a1500pulse, theta2);
           servo5pulse = D2Pminus(a1500pulse, theta3);
           shift(leg);
           buildmove(3, servo3pulse);     //Servo 3 = servo3pulse = theta1
           buildmove(4, servo4pulse);     //Servo 4 = servo4pulse = theta2
           buildmove(5, servo5pulse);          
           finishmove(500);
           delay(500);
         }
         else{
           servo19pulse = D2Pplus(a1500pulse, theta1);
           servo20pulse = D2Pminus(a1500pulse, theta2);
           servo21pulse = D2Pplus(a1500pulse, theta3);
           shift(leg);
           buildmove(19, servo19pulse);     //Servo 3 = servo3pulse = theta1
           buildmove(20, servo20pulse);     //Servo 4 = servo4pulse = theta2
           buildmove(21, servo21pulse);     
           finishmove(500);
           delay(500); 
         }

         } //Next x
        FirstStep = 1; //Tells the program the initial step has taken place                                                     
    } //Next leg

Help? Simple way around the problem?

Please let me know what i can include to better help you guys understand my problem…
I wrote this all by hand, and you think it would be simple enough to do something like this, but everything i try resets all the values, i have to have the current values kept while this is happening… I also really want to keep the number of both local and global variables down

You need to post the whole code, especially if you are wanting to keep values in variables where teh definition of those variables and the location cannot be seen. It may be that you want to define them as static, or make them global, or defined at a differerent context.

        CheckHypotenuse();
           if(CheckHypotenuse()==0){

Why are you calling CheckHypotenuse() twice?

I don't understand why you have a for loop to select the leg to operate on, when the inner loop has completely separate code for each leg.

SilentDemon555:
I cant really explain it, i need the first leg to move, then the second AND as the second one is moving bring the first leg back to Start…

It sounds as if you are trying to control the two legs simultaneously and independently. From your nested loops I think you are trying to do this synchronously. I think you will find this much easier to solve if you do the movement asynchonously, and that is your fundamental problem.

Changing from synchronous to asynchronous would require substantial changes to the structure of your code although a lot of the low level logic would remain the same.

In the asynchronous approach, you would write a piece of code that is called periodically that decides whether the leg needs to be moved yet, and if so moves it. Instead of a FOR loop, this code would use a variable which it increments and tests against a threshold to determine whether the movement has been completed.

Synchronous version can only do one thing at a time:

    for(int step = 0; step < limit; step++)
    {
        position = position + increment;
        leg.move(position);
        delay(stepInterval);
    }
    // carry on with subsequent movements ...

Asynchronous version can do many things concurrently:

    if(millis() - lastStepTime > stepInterval)
    {
        if(step < limit)
        {
            step++;
            position = position + increment;
            leg.move(position);
            lastStepTime = lastStepTime + stepInterval;
        }
        else
        {
            // this movement is complete; change step, stepInterval, increment to start a new movement
        }
    }


    // following code deals with other things you need to move at the same time using code similar to above

For those that asked, here is the monster…

//Inverse Kinematics for Use with Robotic Biped by Quinn and Dave Mikelson
// servos max and min 750 ---- 2100
//L1 = 58mm
//L2 = 45mm

const float pi = 3.14159; //PI Defined
float x, y;  //X and Y Variables used by the IK subroutines
float theta1, theta2, theta3;  //Unknown angles calculated for servos 3,4,5,19,20,21
float l1 = 75;  //length of the connections between each leg
float l2 = 57;  // " "
float servo3pulse, servo4pulse, servo5pulse, servo19pulse, servo20pulse, servo21pulse;  //The microsecond pulses (750 - 2100) used to move servos
float servo1, servo2, servo3, servo4, servo5, servo6, servo17, servo18, servo19, servo20, servo21, servo22, a1500pulse; // Used in the StartPos routine
float fracdone;  // X - Minimum / Maximum = Fraction(0 --- 1) of step completion
int FirstStep = 0; // May be used later, useful for synchronous movement


void setup() {
  
Servos();  // Defines servo starting posistions
    
Serial.begin(9600);

StartPos(); // moves all servos to starting posistions

delay(2000);

Serial.println("READY");

}

void loop() {
//int i;
float xCenter = -30;  // Defined numbers critical for leg movement, currently I still have to use Excel to calculate these
float yCenter = -202.14;
float CirRadius = 119.14;
int leg;
int xmin, xmax, xstep;


  xmin = -80; // User Inputted Min / Max Step Values
  xmax = 20;
  xstep = 10;


    for(leg = 1; leg <= 2; leg++){ // switches the value for leg, Leg = 1, then Leg = 2, Then Leg = 1, etc....
        for(x = xmin; x <= xmax; x = x + xstep){ // runs through all values from min to max based on inverse kinematic equations
        
            y = yCenter + sqrt(sq(CirRadius)-sq(x-xCenter)); // calculates the Y variable, also used in IK2
            
            fracdone = (x - xmin) / (xmax-xmin); // the fracdone equation, used in Shift
            
            CheckHypotenuse();          // I dont know why i did this, an oversight perhaps? I just dont really want to modify things yet (it all sorta works)
              if(CheckHypotenuse()==0){ // if the angle is too far to reach, stop movement
                 return;
               }
             ik2();    // calculates X and Y into Thetas
             normalize(); // Corrects for servo error, and puts servos in correct quadrant
             
             if(leg == 1){ // if First Leg, do only this...
    
               servo3pulse = D2Pminus(a1500pulse, theta1);
               servo4pulse = D2Pplus(a1500pulse, theta2);
               servo5pulse = D2Pminus(a1500pulse, theta3);
               shift(leg);
               buildmove(3, servo3pulse);     //Servo 3 = servo3pulse = theta1
               buildmove(4, servo4pulse);     //Servo 4 = servo4pulse = theta2
               buildmove(5, servo5pulse);          
               finishmove(500);
               delay(500);
             }
             else{ //If Second Leg, do only this...
               servo19pulse = D2Pplus(a1500pulse, theta1);
               servo20pulse = D2Pminus(a1500pulse, theta2);
               servo21pulse = D2Pplus(a1500pulse, theta3);
               shift(leg);
               buildmove(19, servo19pulse);     //Servo 3 = servo3pulse = theta1
               buildmove(20, servo20pulse);     //Servo 4 = servo4pulse = theta2
               buildmove(21, servo21pulse);     
               finishmove(500);
               delay(500); 
             }
    
             } //Next x
            FirstStep = 1; //Tells the program the initial step has taken place                                                     
        } //Next leg
    
    
}
void ik2(){
// Based on Robot_Arm_Designer.xls
float c2, s2, k1, k2;
c2=(x*x+y*y-l1*l1-l2*l2)/(2*l1*l2);
s2=sqrt(1-c2*c2);
k1=l1+l2*c2;
k2=l2*s2;
theta1=atan2(y,x)-atan2(k2,k1);
theta2=atan2(s2,c2);


if (theta1<0){
  theta1 = 2 * pi + theta1;
}
if (theta2<0) {
  theta2 = 2 * pi + theta2;
}

theta2 =(theta2 * (180.0/pi));
theta1 = (theta1 * (180.0/pi));
theta3 = (360 - theta1 - theta2);

}


 int D2Pminus(int hardware, float deg){
   return(hardware - deg * 10);  
 }
 
 int D2Pplus(int hardware, float deg){
   return(hardware + deg * 10);   
 }
 
void buildmove(int servo, int position1) {
  Serial.print(" #");
  Serial.print(servo);
  Serial.print(" P");
  Serial.print(position1);
}

void finishmove(int time) {
  Serial.print(" T");
  Serial.println(time);
} 
 
 void check(){
        if (servo3pulse <= 750) servo3pulse = 750;
   else if (servo3pulse >= 2100) servo3pulse = 2100;
   else if (servo4pulse <= 750) servo4pulse = 750;
   else if (servo4pulse >= 2100) servo4pulse = 2100;
   else if (servo19pulse <= 750) servo19pulse = 750;
   else if (servo19pulse >= 2100) servo19pulse = 2100;
   else if (servo20pulse <= 750) servo20pulse = 750;
   else if (servo20pulse >= 2100) servo20pulse = 2100;
 
 } 

int CheckHypotenuse(){                    //Returns 1 if Target is Within Reach, 0 if Target Out of Reach
float hypotenuse;

hypotenuse = sqrt(sq(x) + sq(y));

if ((hypotenuse - l1 -l2) > 0.001) {
  Serial.print("hypotenuse ="); 
  Serial.println(hypotenuse); 
  Serial.print("This is greater than");
  Serial.println(l1 + l2); 
return(0);
}
  else 
    return(1);
  
}

void normalize(){
 theta1 = theta1 - 270;
 theta2 = theta2 - 90; 
 theta3 = theta3 - 75;
  
}

void shift(int leg){
int rollfootpulse; 
  
  if(fracdone == 0){
    int angle = 15;
    
        if(leg == 1){
          
          rollfootpulse = D2Pplus(servo6, angle);
            buildmove(6, rollfootpulse);
          rollfootpulse = D2Pplus(servo2, angle);  
            buildmove(2, rollfootpulse);
          rollfootpulse = D2Pplus(servo22, angle);
            buildmove(22, rollfootpulse);
          rollfootpulse = D2Pplus(servo18, angle);
            buildmove(18, rollfootpulse);
          finishmove(800);
          return;
        }
       else if(leg == 2){
         rollfootpulse = D2Pminus(servo6, angle + 2);
            buildmove(6, rollfootpulse);
          rollfootpulse = D2Pminus(servo2, angle + 2);  
            buildmove(2, -rollfootpulse);
          rollfootpulse = D2Pminus(servo22, angle + 2);
            buildmove(22, rollfootpulse);
          rollfootpulse = D2Pminus(servo18, angle + 2);
            buildmove(18, -rollfootpulse);
          finishmove(800);
         
          return;
        }
    } 
  
  if(fracdone == .8){
    
     int angle = 0;
     
        if(leg == 1){
          rollfootpulse = D2Pplus(servo6, angle);
            buildmove(6, rollfootpulse);
          rollfootpulse = D2Pplus(servo2, angle);  
            buildmove(2, -rollfootpulse);
          rollfootpulse = D2Pplus(servo22, angle);
            buildmove(22, rollfootpulse);
          rollfootpulse = D2Pplus(servo18, angle);
            buildmove(18, -rollfootpulse);
          finishmove(800);
          
          return;
        }
       else if(leg == 2){
         rollfootpulse = D2Pminus(servo6, angle);
            buildmove(6, rollfootpulse);
          rollfootpulse = D2Pminus(servo2, angle);  
            buildmove(2, -rollfootpulse);
          rollfootpulse = D2Pminus(servo22, angle);
            buildmove(22, rollfootpulse);
          rollfootpulse = D2Pminus(servo18, angle);
            buildmove(18, -rollfootpulse);
          finishmove(800);
        
          return;
        }
    }   
   

}
void Servos(){
  
servo1 = 1500;
servo2 = 1540;
servo3 = 1900;
servo4 = 1300;
servo5 = 1700;
servo6 = 1550;

servo17 = 1500;
servo18 = 1650;
servo19 = 1100;
servo20 = 1700;
servo21 = 1300;
servo22 = 1550;

a1500pulse = 1500;
return;
}

void StartPos(){
  buildmove(1, servo1);    
  buildmove(2, servo2);
  buildmove(3, servo3); 
  buildmove(4, servo4);    
  buildmove(5, servo5);
  buildmove(6, servo6); 
  
  buildmove(17, servo17);    
  buildmove(18, servo18);
  buildmove(19, servo19); 
  buildmove(20, servo20);    
  buildmove(21, servo21);
  buildmove(22, servo22); 
  finishmove(500);
}

I've done some commenting, but it is still pretty hairy, and yes, there may be some redundancies. I understand i might need to switch to asynchronous leg control, but it would be way more difficult than just saying leg.move, i get this was supposed to be an example, but i don't quite understand all of the changes that would need to be made. Please understand this is a 12 servo biped, and it is extremely difficult to modify code because each servo relies on a variable given to another servo... Therefore it would be a daunting task to start duplicating variables in order to separate the legs...

SilentDemon555: Therefore it would be a daunting task to start duplicating variables in order to separate the legs...

You need to abstract out the complexity.

If you have a number of legs that all behave similarly, create a class to manage one leg and then create as many of those as you need. If the control logic for one servo needs to send commands or share state with the control logic for another servo, configure the master servo with details of the slave servo it is to control. If your control logic needs to have a servo move to a specified position in slow time, create a class that controls a single servo that encapsulates the logic to decide when to adjust the position and by how much so that you can just keep running that code for all servos and they will all move themselves as required. Then your code looks like: an overall manager class that knows how to move the device by moving the legs. A manager for each leg that knows how to do an action with that leg ("put your foot here") by flexing the joints in that leg. A controller for each joint / servo that knows how to move the joint at a given speed and direction.