Let's Make Robots!

6 legged walker

As some of you know I'm going to built a small hexapod using 12 9G servos (2 DOF/leg). I have the electronics figured out, I will use my Bajduino 3A pcb (ATmega328 with 3A 5V vreg). I'm now waiting for the acryl parts so I can built my little hexapod. So I've been thinking how I'm going to program it :) Understand that I know very little about programming... So tonight I found a sketch on my harddrive named 6 legged walker. I think that I've downloaded it from this very forum. But I can not seem to find the robot running this sketch. The thing about this sketch is that I have no clue how it really works, but it looks very interesting :)

This is the sketch:

#include <IRremote.h>
#include <IRremoteInt.h>

#include <Servo.h>

int svc[12]={                                              // servo center positions
  1500,1550,1550,1450,                                     // D29 knee1, D46 thigh1, D47 knee2, D48 thigh2
  1500,1400,1500,1550,                                     // D49 knee3, D50 thigh3, D51 knee4, D24 thigh4
  1500,1500,1500,1400                                      // D25 knee5, D26 thigh5, D27 knee6, D28 thigh6
};

int svp[12];                                               // servo current position
int svs[12];                                               // servo speed (how much to increment position each loop)
Servo sv[12];                                              // Yes! servos can be an array

int angle;
int rotate;
int Speed;
int Stride=400;

int RECV_PIN = 10;
IRrecv irrecv(RECV_PIN);
decode_results results;



void setup()
{
  pinMode(11,OUTPUT);
  pinMode(12,OUTPUT);
  digitalWrite(11,0);
  digitalWrite(12,1);
  
  for(int i=0;i<12;i++)                                    // copy center positions into servo position array
  {
    svp[i]=svc[i];   
  }
  
  sv[0].attach(29,800,2200);                               // knee  1 
  delay(40);
  sv[1].attach(46,800,2200);                               // thigh 1
  delay(40);
  sv[2].attach(47,800,2200);                               // knee  2
  delay(40);
  sv[3].attach(48,800,2200);                               // thigh 2
  delay(40);
  sv[4].attach(49,800,2200);                               // knee  3
  delay(40);
  sv[5].attach(50,800,2200);                               // thigh 3
  delay(40);
  sv[6].attach(51,800,2200);                               // knee  4
  delay(40);
  sv[7].attach(24,800,2200);                               // thigh 4
  delay(40);
  sv[8].attach(25,800,2200);                               // knee  5
  delay(40);
  sv[9].attach(26,800,2200);                               // thigh 5
  delay(40);
  sv[10].attach(27,800,2200);                              // knee  6
  delay(40);
  sv[11].attach(28,800,2200);                              // thigh 6
  delay(40);
  
  for(int i=0;i<12;i++)
  {
    sv[i].writeMicroseconds(svp[i]);                       // initialize servos
  }
  delay(3000);
  
  Serial.begin(57600);
  irrecv.enableIRIn();                                     // Start the receiver
}

void loop()
{ 
  if (irrecv.decode(&results))
  {
    int IRC=results.value;
    if(IRC==2640 || IRC==528)                              // STOP
    {
      Speed=0;
      angle=0;
      rotate=0;
    }
    
    if(IRC==144 || IRC==2064)                              // FORWARD
    {
      Speed=10;
      rotate=0;
      angle=0;
    }
    
    if(IRC==2192 || IRC==3600)                             // REVERSE    
    {
      Speed=-10;
      rotate=0;
      angle=0;
    }
    
    if(IRC==1168)                                          // ROTATE CLOCKWISE  
    {
      Speed=10;
      rotate=1;
      angle=0;
    }
    
    if(IRC==3216)                                          // ROTATE COUNTER CLOCKWISE  
    {
      Speed=10;
      rotate=-1;
      angle=0;
    }
    
    if(IRC==16)                                            // 45 DEGREES    
    {
      Speed=10;
      rotate=0;
      angle=45;
    }
    
    if(IRC==3088)                                          // 90 DEGREES    
    {
      Speed=10;
      rotate=0;
      angle=90;
    }
    
    if(IRC==1552)                                          // 135 DEGREES    
    {
      Speed=10;
      rotate=0;
      angle=135;
    }
    
    if(IRC==272)                                           // 225 DEGREES    
    {
      Speed=10;
      rotate=0;
      angle=225;
    }
    
    if(IRC==2576)                                          // 270 DEGREES    
    {
      Speed=10;
      rotate=0;
      angle=270;
    }
    
    if(IRC==1040)                                          // 315 DEGREES    
    {
      Speed=10;
      rotate=0;
      angle=315;
    }
    
    irrecv.resume(); // Receive the next value
  }
    
  if (angle<0) angle+=360;
  if (angle>359) angle-=360;
  Walk();
  delay(15);
}


void Walk()
{
  float A;
  double Xa,Xs;
  double Ya,Ys;
  static int Step;
  
  for(int i=0;i<6;i+=2)
  {
    A=float(60*i+angle);
    if(A>359) A-=360;
   
    A=A*PI/180;                                    // convert degrees to radians
    Xa=Stride*rotate;
    if(rotate==0)
    {
      Xa=sin(A)*-Stride;
    }
        
    A=float(Step);
    A=A*PI/180;                                    // convert degrees to radians
    Xs=sin(A)*Stride;
    Ys=cos(A)*Xa;
    
    sv[i*2].writeMicroseconds(svc[i*2]+int(Xs));
    sv[i*2+1].writeMicroseconds(svc[i*2+1]+int(Ys));
  }
  
  for(int i=1;i<6;i+=2)
  {
    A=float(60*i+angle);
    if(A>359) A-=360;
   
    A=A*PI/180;                                    // convert degrees to radians
    Xa=Stride*rotate;
    if(rotate==0)
    {
      Xa=sin(A)*-Stride;
    }
        
    A=float(Step+180);
    if(A>359) A-=360;
    A=A*PI/180;                                    // convert degrees to radians
    Xs=sin(A)*Stride;
    Ys=cos(A)*Xa;
    
    sv[i*2].writeMicroseconds(svc[i*2]+int(Xs));
    sv[i*2+1].writeMicroseconds(svc[i*2+1]+int(Ys));
  }
  
  Step+=Speed;
  if (Step>359) Step=0;
  
}

I assume the void walk sets the servo position in little steps, and the delay of 15 milliseconds
is enough time to move the servo's? What I really can't figure out is the calculation of the servo position.
In the beginning of the sketch an integer "stride" is set to 400, what is this value?
Anyone that can explain how the calculation of the servo angle works?

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.

The code looks like it uses inverse kinematics. I didn't know about that until now. Found this interesting link: http://arduin0.blogspot.be/2012/01/inverse-kinematics-ik-implementation.html If any one has more information about inverse kinematics please don't hesitate to share it...

It does not look easy to implement. I will need to brush up my math :)

Looks like you can adjust the length of step taken just by changing the variable stride. Very handy but I imagine there will be a sweet range where it walks perfect and smaller or bigger will make it unstable or un-natural.
Not sure I can explain the math for servo angle though.

Updated my original post to make it more readable :)

Hint with your code. Stick it on pastebin, I believe the C syntax highlight mode suffices on arduino code. Its much more readable with the original character spacing preserved (which pastebin does, infact its meant for hosting code snippets)

...or do it this way: In the Arduino IDE -> Edit -> Copy as HTML.

Read this for more information.