Biped Coding Issues

Using Arduino Programming Questions
1

So I've been building a twelve servo biped for quite some time, and run into more than a handful of problems. I posted a very similar question, in fact same, asking for help, and I got some, but couldn't focus on the robot and set it aside for upwards of two months. I've come back and hit the same roadblock I had before...

My original question at For-Loop Questions and a Complex Biped - #6 by SilentDemon555 - Programming Questions - Arduino Forum
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...

I need to be able to modify values of the legs "on-the-fly, and have the legs move, in the words of PeterH, in an asynchronous fashion.
I got some basic ideas, with no idea what to change or where to change it... I'm so lost and have no idea where to start converting this "synchronous movement" into "asynchronous movement"

Once again here's the main part of the FOR-LOOP in the code...

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
2

Hello,

I suggest you start from scratch again! Put the complex maths aside for now, concentrate on the logic first, use lots of Serial.print to debug your logic until it does exactly what you want, only then you put the maths in :slight_smile:

Also some tips:

Use functions arguments, for example not ik2() but ik2(x, y).
Use understandable variables/functions names.
Do not use delay().

3

Do as guix suggests. In particular, learn to program without using delay(). Your loop() function should probably have the following form:

if (elapsed time since we last moved the servos >= T)
{
make a note of the current time;
move the servos to the new positions we have already calculated for them;
calculate and store the positions we want to move them to when another time interval of length T has elapsed;
}
else
{
do nothing;
}

No for-loops or delay calls.

4

Thank you both for the response, I don't quite understand the need for an entire rewrite of my code, the IK function (Inverse Kinematic Function Set) does not need to call for x and y because of the global variables in the "Control Box." I also don't understand the difference between For-Loop and the milli counter. Doesn't it do the same thing? Again, I only call Delay three times, one to allow me to position the robot before it starts moving, and twice to allow the legs to move to their position before starting again... is this where i need to rid the code of a delay?

If I do use the millis setup, how does that allow me to move the legs whenever i want, even if a leg is already moving, (how does it allow me to control each leg interdependently?)

I am a single person and have a very average coding skill. This code set has taken me around four months to perfect(incorrectly) so a complete rewrite would be almost impossible for me to undertake.

In the effort to give as much info as possible, here is the code...

5 
//Inverse Kinematics for Use with Robotic Biped by Quinn 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 IK
            
            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;
               }
             IK();    // 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 IK(){
// Based on Robot_Arm_Designer.xls
float c2, s2, k1, k2;
c2=(sq(x)+sq(y)-sq(l1)-sq(l2))/(2*l1*l2);
s2=sqrt(1-sq(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);
}
6

If you want to control and/or respond to just one device, then using delay is fine. When you want to control and/or respond to several devices simultaneously, then you need to learn to program without using delay calls.

Study the blinkWithoutDelay example. Then extend it to blink 2 or 3 LEDs at different rates. Then you may be ready to think about how to control 12 servos.

7

So I've looked at the code and tried to figure out a way to implement the millis functions, and I know you guys are giving me a very straightforward answer. I guess I'm just not getting it, i understand that it says do something (or don't) after a given interval period and then reset... but that still seems exactly the same as "do something only if Leg = 1" the millis just seems to say "if one second has passed move leg 1" I'm just not seeing benefit... I'm sorry i'm being so stubborn, and i'm really looking for a way to fix this problem.

On the physical side:
The robot needs to take a half-step at the start, and then take full, long, alternating steps. Right now the robot takes a step, then takes another, gets extremely unbalanced, jerks the first foot back, and falls over. I need to be able to change the foot patterns using a set of fairly uncomplicated code. I know it would be easy to write a hundred different functions to call individual steps, but i want re-configurable code. The robot has to run all x and y values through the IK code, translate that into servo positions and then at the end of the full step, the foot has to start moving back as the second foot starts moving forward. Shifting weight is handled in the shift function.

I don't know where to start changing stuff and really can't afford a true rewrite of the code... i guess i'm just being lazy.
Thank you so much for any insight into a solution

8

Could I just use different interval variables to call different moves?
IE: Interval1 = 1 second
Interval2 = 1.5 seconds

Move leg 1 on interval1
Move leg 2 on interval2

Also what is the best way to move all the servos back to the starting position without just jumping there?

9

SilentDemon555:
Could I just use different interval variables to call different moves?
IE: Interval1 = 1 second
Interval2 = 1.5 seconds

Move leg 1 on interval1
Move leg 2 on interval2

You could, but if you are not very careful then they may get out of sync. Better to use an interval of 0.5 seconds, and schedule leg 1 to move after 2 intervals and leg 2 to move after 3 intervals.

Have you considered something like the following:

  • in setup() move all servos to their starting positions. You can use delay calls here to avoid driving them all at the same time, if you need to do this to avoid overloading your power supply.
  • break the walking cycle (i.e. one pair of steps) down into equally spaced time intervals, where at each point you move just a small number of servos (perhaps just one) or do nothing
  • construct a table to specify at each point in the cycle which servo(s) you need to move and where you need to move them to
  • then loop() can just iterate through the table, waiting until the appropriate time before moving on to the next point in the cycle
10

Originally my code did that, like making a stop motion animation, but what happens when you hit an incline? The answer simply is that it stops working. That's why I translated real world Inverse Kinematic Algorithm Functions into code for the Arduino, and I spent a month after perfecting it. The codes function is to predict all of the positions the servos need to be in at the exact time they should be.
Now if the IMU(not implemented yet) says that you have reached an incline, instead of having to rework all of the servo pulses(a nightmare that I've dealt with before) the robot can self-modify it's X, Y, and Z values to angle it's feet. It would be almost impossible to move just one servo at a time...
I technically only need 3 servos to walk, but without the 3 other "balance servos" moving to compensate, it would fail to move, or worse, fall.

The delay calls allow the SSC-32 Servo controller enough time to move all the servos to their given destinations before the code restarts.

This is almost what my robot looks like, I don't have a camera on me, and I've made some pretty big changes since I took these photos...

Scout %284%29.JPG
Scout %284%29.JPG566×640 60.8 KB

11

So use the table to store what the servo angles would be on the flat, but before writing them to the servos, adjust them according to the incline.

Or don't use table lookup at all, but instead calculate the servo motion required, knowing where you are in the cycle, the current servo angles, and the incline.

12

Thank you, I have a general idea on where to start, I can calculate where i am in the cycle with my fracdone function, but still don't see a way around using a for loop to find servo positions. I'll start playing with it to see what I come up with.

13

By all means use a for-loop to calculate servo positions. But don't send commands to servos in that loop, or use delay calls.