Quadruped robot foot sequence

Using Arduino Programming Questions
1

hi forum
i build quadruped arduino robot from hexapod robot from this link and succeed to modified the code as my needs since my servo position isn't the same relative to the servo bracket as the original project. my quadruped robot legs order is : leg 1 and 2 on the right side of the robot and leg 3 and 4 on the left side, leg 1 and 3 are front leg. all the legs forward movement run as my expectation but they step(run) at the same time. how do i modified my code in for loop to make leg 1 and 4 to step forward and when those legs step down leg 2 and 3 step forward. thank you

this is the code


#include <Servo.h>
Servo s18;
Servo s17;
Servo s16;
Servo sc1;  // create servo object to control a servo
Servo sf1;
Servo st1;
Servo sc2;  
Servo sf2;
Servo st2;
Servo sc3;  
Servo sf3;
Servo st3;
Servo sc4;  
Servo sf4;
Servo st4;
int i0H1 = 0;


int i1L1 = 0;
int i2L1 = 0;
int i3L1 = 0;
int i4L1 = 0;
int i5L1 = 0;
int i6L1 = 0;

int i1L2 = 0;
int i2L2 = 0;
int i3L2 = 0;
int i4L2 = 0;
int i5L2 = 0;
int i6L2 = 0;
boolean l1status = LOW;
boolean l2status = LOW;
boolean aStatus = LOW;
boolean attStatus = LOW;
int k = 0;
int a = 0;
int aa = 0;
int period = 1000;
unsigned long time_now = 0;

boolean c = true;
float distance;
long duration;
int dataIn;
int m = 0;
int h = 0;
int t = 0;
int att = 0;
int speedV = 50;

void setup() {

  Serial.begin(15200);

sc1.attach(37);  // 
sf1.attach(38);
st1.attach(39);
sc2.attach(40);
sf2.attach(41);
st2.attach(42);
sc3.attach(33);
sf3.attach(32);
st3.attach(31);
sc4.attach(30);
sf4.attach(29);
st4.attach(28);

sc1.write(70);
sf1.write(90);
st1.write(90);//tibia 1
sc2.write(70);
sf2.write(90);
st2.write(100); // tibia 2
sc3.write(100);
sf3.write(100);
st3.write(90);// tibia 3
sc4.write(100);
sf4.write(100);
st4.write(90);// tibia 4

  delay(3000);

  delay(4000);
}
void loop() {

Serial.begin(9600);
     moveLeg3();
      moveLeg2();
    moveLeg1();
    moveLeg4();
    delay(speedV);
}

void moveLeg1() { 
  //rise the tibia
  if (i1L1 <= 10) {
    st1.write(90 - i1L1 * 2);
    sf1.write(90 - i1L1 * 3);
    i1L1++;
  }
  // rotate coax
  if (i2L1 <= 30) {
    sc1.write(70 + i2L1);
    i2L1++;

  }
  // step down the tibia
  if (i2L1 > 20 & i3L1 <= 10) {
    st1.write(70 + i3L1 * 2);
    sf1.write(60 + i3L1 * 3);
    i3L1++;
  }
  // totate coax back
  if (i2L1 >= 30) {
    sc1.write(100 - i4L1);
    i4L1++;
    l1status = HIGH;
  }
  if (i4L1 >= 30) {
    i1L1 = 0;
    i2L1 = 0;
    i3L1 = 0;
    i4L1 = 0;
    i5L1 = 0;
  }
  }

void moveLeg2() {
  // lift tibia
   if (i1L1 <= 10) {
    st2.write(90 - i1L1 * 2);
    sf2.write(90 - i1L1 * 3);
    i1L1++;
  }
  //move coax
  if (i2L1 <= 30) {
    sc2.write(70 + i2L1);
    i2L1++;

  }
  // step down tibia
  if (i2L1 > 20 & i3L1 <= 10) {
    st2.write(70 + i3L1 * 2);
    sf2.write(60 + i3L1 * 3);
    i3L1++;
  }
  // rotate back the coax
  if (i2L1 >= 30) {
    sc2.write(100 - i4L1);
    i4L1++;
  }
  if (i4L1 >= 30) {
    i1L1 = 0;
    i2L1 = 0;
    i3L1 = 0;
    i4L1 = 0;
    i5L1 = 0;
  }
}

void moveLeg3() { 
 if (i1L2 <= 10) {
    st3.write(90 + i1L2 * 2);
    sf3.write(100 + i1L2 * 3);
    i1L2++;
  }
  if (i2L2 <= 30) {
    sc3.write(100 - i2L2);
   i2L2++;
  }
  if (i2L2 > 20 & i3L2 <= 10) {
    st3.write(110 - i3L2 * 2);
    sf3.write(120 - i3L2 * 3);
     i3L2++;
  }
  if (i2L2 >= 30) {
    sc3.write(70 + i4L2);
    i4L2++;
    l1status = LOW ;
  }
  if (i4L2 >= 30) {
    i1L2 = 0;
    i2L2 = 0;
    i3L2 = 0;
    i4L2 = 0;
    i5L2 = 0;
  }
  }


void moveLeg4() {
    if (i1L2 <= 10) {
    st4.write(90 + i1L2 * 2);
    sf4.write(100 + i1L2 * 3);
    i1L2++;
  }
  if (i2L2 <= 30) {
    sc4.write(100 - i2L2);
   i2L2++;
  }
  if (i2L2 > 20 & i3L2 <= 10) {
    st4.write(110 - i3L2 * 2);
    sf4.write(120 - i3L2 * 3);
     i3L2++;
  }
  if (i2L2 >= 30) {
    sc4.write(70 + i4L2);
    i4L2++;
  }
  if (i4L2 >= 30) {
    i1L2 = 0;
    i2L2 = 0;
    i3L2 = 0;
    i4L2 = 0;
    i5L2 = 0;
  }
}



void initialPosition() {
  a = 0;
  aa = 0;
  m = 0;
  l1status = LOW;
  l2status = LOW;

  // Leg 1
  st1.write(90);// tbia 1
  sf1.write(90);
  sc1.write(70);
  st2.write(90);// tibia 2
  sf2.write(90);
  sc2.write(70);
  st3.write(90);//tibia 3
  sf3.write(100);
  sc3.write(100);
  st4.write(90);//tibia 4
  sf4.write(100);
  sc4.write(100);

  i1L1 = 0;
  i2L1 = 0;
  i3L1 = 0;
  i4L1 = 0;
  i5L1 = 0;
  i6L1 = 0;

  i1L2 = 0;
  i2L2 = 0;
  i3L2 = 0;
  i4L2 = 0;
  i5L2 = 0;
  i6L2 = 0;
}
2

Please tell us what you have tried and what were the results.
Paul

3

In order to successfully modify the code you downloaded, you first need to understand it.

Start by going through the code line by line, making sure that you understand what each line does, and why. Then you will be in a position to make changes.

4

Some really good development aids for you to study, would be to implement arrays, structs, and eventually a state machine.

The first two will help you better organise your code, a well designed state machine will allow you to modify the gait and distribution of load in real-time across the limbs. This in turn will help as you address balancing and turning the quadruped.

5

Give the servo's meaningful names. I don't know what your robot legs look like, but for example:

Servo rightLegKnee;
Servo rightLegAnkle;
Servo leftLegKnee;

only use single letter variables like

int m = 0;
int h = 0;
int t = 0;

inside the scope of very small code blocks, for example small 'for' 'while' or 'if' statements, or short functions. If they have any special meaning, give them full length names.

Doubtless this would help you maintain the code, when you have to make changes such as the one that you're stuck with now...

6

By wrapping each set of leg servos in an object, you only need one function to move all of the legs forward or back to any position in the step cycle. The cycle is based on the Up and Down positions for two servos and the Front and Back positions for the other. These six positions can be different for each leg.

#include <Servo.h>

class Leg
{
  public:
    Leg(const byte TPin, const byte FPin, const byte CPin) : TPin(TPin), FPin(FPin), CPin(CPin) {}
    void begin()
    {
      TServo.attach(TPin);
      FServo.attach(FPin);
      CServo.attach(CPin);
    }

    void setTPositions(int Up, int Down)
    {
      TUpPosition = Up; TDownPosition = Down; TChange = TUpPosition - TDownPosition;
      TServo.write(TDownPosition);
    }

    void setFPositions(int Up, int Down)
    {
      FUpPosition = Up; FDownPosition = Down; FChange = FUpPosition - FDownPosition;
      FServo.write(FDownPosition);
    }

    void setCPositions(int Back, int Front)
    {
      CBackPosition = Back; CFrontPosition = Front; CChange = CFrontPosition - CBackPosition;
      CServo.write(CBackPosition);
    }

    void move(byte portion);  // 0-255 for forward step. 255-0 for backward step.

  private:
    const byte TPin, FPin, CPin;
    Servo TServo, FServo, CServo;

    int TDownPosition, TUpPosition, TChange;
    int FDownPosition, FUpPosition, FChange;
    int CBackPosition, CFrontPosition, CChange;
};


// Pass a value from 0 to 255 to position the leg
// within a stride
void Leg::move(byte portion)
{
  // 0-42: Lift the foot
  if (portion <= 42)
  {
    TServo.write(TDownPosition + TChange * portion / 42);
    FServo.write(FDownPosition + FChange * portion / 42);
  }

  // 0-128: Rotate the leg forward.
  if (portion <= 128)
  {
    CServo.write(CBackPosition + CChange * portion / 128);
  }

  // 128 through 170 (42 steps): Put down the foot
  if (portion > 128 && portion < 170)
  {
    TServo.write(TUpPosition - TChange * (portion - 128) / 42);
    FServo.write(FUpPosition - FChange * (portion - 128) / 42);
  }

  // 128 through 255: Rotate the leg back
  if (portion > 128)
  {
    CServo.write(CFrontPosition - CChange * (portion - 128) / 128);
  }
}

const unsigned long MillisecondsPerStep = 1000;  // One step per second.
const unsigned long StepRateDelay = MillisecondsPerStep / 256;

// Assign T, F, and C servo pins to each leg
Leg LegFR(39, 38, 37);
Leg LegBR(42, 41, 40);
Leg LegFL(31, 32, 33);
Leg LegBL(28, 29, 30);

void setup()
{
  LegFR.setTPositions(90, 70);
  LegFR.setFPositions(90, 60);
  LegFR.setCPositions(70, 100);
  LegFR.begin();


  LegBR.setTPositions(90, 70);
  LegBR.setFPositions(90, 60);
  LegBR.setCPositions(70, 100);
  LegBR.begin();


  LegFL.setTPositions(90, 110);
  LegFL.setFPositions(90, 120);
  LegFL.setCPositions(70, 100);
  LegFL.begin();


  LegBL.setTPositions(90, 110);
  LegBL.setFPositions(90, 120);
  LegBL.setCPositions(70, 100);
  LegBL.begin();

  // Stand still for a few seconds
  delay(3000);
}

void loop()
{
  // Take 5 steps forward

  // Start with half a step on BL and FR legs
  for (int i = 0; i < 128; i++)
  {
    LegFR.move(i);
    LegBL.move(i);
    delay(StepRateDelay);
  }

  // Then take 5 full steps on all legs:
  for (int i = 0; i < 5 * 255; i++)
  {
    LegFL.move(i);
    LegFR.move(i + 128);
    LegBL.move(i + 128);
    LegBR.move(i);
    delay(StepRateDelay);
  }

  // Then complete the step on the first legs:
  for (int i = 128; i < 256; i++)
  {
    LegFR.move(i);
    LegBL.move(i);
    delay(StepRateDelay);
  }

  // Pause
  delay(2000);
}
1 Like
7

shouldn't there be a sequence of angles for each servo on front and rear each legs, separate sequences for front/rear.

each iteration of loop, the servos on each leg should be set to one set of values in that sequence and each would would start at a different point in the sequence

i ran the code, printing the servo angles. the value may be the sequences described above. i renumbered the pins for each servo

   30: 100 100  98  96  94  92  90  88  86  84  82  80  78  76  74  72  71  73  75  77  79  81  83  85  87  89  91  93  95  97  99  99  97  95  93  91  89  87  85  83
   31: 100 100 106 112 118 124 130   0   0   0   0 120 114 108 102  96  90   0   0   0   0   0   0   0   0   0   0   0   0   0   0 103 109 115 121 127   0   0   0   0
   32:  90  90  94  98 102 106 110   0   0   0   0 110 106 102  98  94  90   0   0   0   0   0   0   0   0   0   0   0   0   0   0  92  96 100 104 108   0   0   0   0

   40:  70  70  72  74  76  78  80  82  84  86  88  90  92  94  96  98  99  97  95  93  91  89  87  85  83  81  79  77  75  73  71  71  73  75  77  79  81  83  85  87
   41:  90  90  84  78  72  66  60   0   0   0   0  60  66  72  78  84  90   0   0   0   0   0   0   0   0   0   0   0   0   0   0  87  81  75  69  63   0   0   0   0
   42: 100  90  86  82  78  74  70   0   0   0   0  70  74  78  82  86  90   0   0   0   0   0   0   0   0   0   0   0   0   0   0  88  84  80  76  72   0   0   0   0

   20: 100  99  97  95  93  91  89  87  85  83  81  79  77  75  73  70  72  74  76  78  80  82  84  86  88  90  92  94  96  98 100  98  96  94  92  90  88  86  84  82
   21: 100 103 109 115 121 127   0   0   0   0   0 117 111 105  99  93   0   0   0   0   0   0   0   0   0   0   0   0   0   0 100 106 112 118 124 130   0   0   0   0
   22:  90  92  96 100 104 108   0   0   0   0   0 108 104 100  96  92   0   0   0   0   0   0   0   0   0   0   0   0   0   0  90  94  98 102 106 110   0   0   0   0

   10:  70  71  73  75  77  79  81  83  85  87  89  91  93  95  97 100  98  96  94  92  90  88  86  84  82  80  78  76  74  72  70  72  74  76  78  80  82  84  86  88
   11:  90  87  81  75  69  63   0   0   0   0   0  63  69  75  81  87   0   0   0   0   0   0   0   0   0   0   0   0   0   0  90  84  78  72  66  60   0   0   0   0
   12:  90  88  84  80  76  72   0   0   0   0   0  72  76  80  84  88   0   0   0   0   0   0   0   0   0   0   0   0   0   0  90  86  82  78  74  70   0   0   0   0
8

hi all brother. i succeed to modified the code suit my hardware (robot frame) condition. i manage the quadruped to be able just to move forward and here is the code.

#include <Servo.h>
Servo sc1;  // create servo object to control a servo
Servo sf1;
Servo st1;
Servo sc2;  
Servo sf2;
Servo st2;
Servo sc3;  
Servo sf3;
Servo st3;
Servo sc4;  
Servo sf4;
Servo st4;

int i1L1 = 0;
int i2L1 = 0;
int i3L1 = 0;
int i4L1 = 0;
int i5L1 = 0;

int i1L2 = 0;
int i2L2 = 0;
int i3L2 = 0;
int i4L2 = 0;
int i5L2 = 0;

boolean l1status = LOW;

int speedV = 40;

void setup() {

 Serial.begin(15200);

sc1.attach(37);  
sf1.attach(38);
st1.attach(39);
sc2.attach(40);
sf2.attach(41);
st2.attach(42);
sc3.attach(33);
sf3.attach(32);
st3.attach(31);
sc4.attach(30);
sf4.attach(29);
st4.attach(28);

sc1.write(70);
sf1.write(85);
st1.write(94);//tibia 1
sc2.write(70);
sf2.write(90);
st2.write(90); // tibia 2
sc3.write(90);
sf3.write(95);
st3.write(90);// tibia 3
sc4.write(100);
sf4.write(86);/
st4.write(82);/tibia 4

  delay(4000);
}
void loop() {

   moveLeg1();
    moveLeg4();
    if (l1status == HIGH) {
      moveLeg2();
      moveLeg3();
    
  }
    delay(speedV);
}

void moveLeg1() { 
  //rise the tibia
  if (i1L1 <= 10) {
    st1.write(98 - i1L1 * 2);
    sf1.write(90 - i1L1 * 3);
    i1L1++;
  }
  // rotate coax forward
  if (i2L1 <= 30) {
    sc1.write(70 + i2L1);
    i2L1++;

  }
  // step down the tibia
  if (i2L1 > 20 & i3L1 <= 10) {
    st1.write(87 + i3L1 * 2);//70
    sf1.write(60 + i3L1 * 3);//60
    i3L1++;
  }
  // rotate coax back
  if (i2L1 >= 30) {
    sc1.write(100 - i4L1);
    i4L1++;
    l1status = HIGH;
  }
  if (i4L1 >= 30) {
    i1L1 = 0;
    i2L1 = 0;
    i3L1 = 0;
    i4L1 = 0;
    i5L1 = 0;
  }
  }

void moveLeg2() {
  
   if (i1L2 <= 10) {
    st2.write(90 - i1L2 * 2);
    sf2.write(80 - i1L2 * 3);
    i1L2++;
  }
  
  if (i2L2 <= 30) {
    sc2.write(70 + i2L2);
    i2L2++;
  }
  
  if (i2L2 > 20 & i3L2 <= 10) {
    st2.write(70 + i3L2 * 2);
    sf2.write(60 + i3L2 * 3);
    i3L2++;
  }

  if (i2L2 >= 30) {
    sc2.write(100 - i4L2);
    i4L2++;
  }
  
  if (i4L2 >= 30) {
    i1L2 = 0;
    i2L2 = 0;
    i3L2 = 0;
    i4L2 = 0;
    i5L2 = 0;
  }
}

void moveLeg3() { 
  
 if (i1L2 <= 10) {
    st3.write(83 + i1L2 * 2);
    sf3.write(90 + i1L2 * 3);
    i1L2++;
  }
  
  if (i2L2 <= 30) {
    sc3.write(100 - i2L2);
   i2L2++;
  }
  
  if (i2L2 > 20 & i3L2 <= 10) {
    st3.write(90 - i3L2 * 2);
    sf3.write(110 - i3L2 * 3);
     i3L2++;
  }
  
  if (i2L2 >= 30) {
    sc3.write(70 + i4L2);
    i4L2++;
  
  }
  
  if (i4L2 >= 30) {
    i1L2 = 0;
    i2L2 = 0;
    i3L2 = 0;
    i4L2 = 0;
    i5L2 = 0;
  }
  }

void moveLeg4() {
  
    if (i1L1 <= 10) {
    st4.write(70 + i1L1 * 2);
    sf4.write(75 + i1L1 * 3);
    i1L1++;
  }
  
  if (i2L1 <= 30) {
    sc4.write(100 - i2L1);
   i2L1++;
  }
  
  if (i2L1 > 20 & i3L1 <= 10) {
    st4.write(75 - i3L1 * 2);
    sf4.write(95 - i3L1 * 3);
     i3L1++;
  }
  
  if (i2L1 >= 30) {
    sc4.write(70 + i4L1);
    i4L1++;
  }
  if (i4L1 >= 30) {
    i1L1 = 0;
    i2L1 = 0;
    i3L1 = 0;
    i4L1 = 0;
    i5L1 = 0;
  }
}

the result of my code is leg 1 and 4 do forward move and exactly when those leg do backward move the leg 2 and 3 do forward move . but when i try to launch the robot on the ground its always fail ( its seem all the servo lose power). i use lipo 2s 35c 5200mah and the step down module power xl4016 which is able to continuous burst 5 amp. my suggest may be the module unable to supply current needed by the robot to move 2 legs each time or my battery is broken. my second plan to create another gait ex. to move each leg one at a time. can anyone of you help me to change the code to meet that gait? thank you in advance

9

brother your code is advanced to me, and i will try it.

10

How would that work? With the three other legs planted on the ground, running one leg through a cycle is likely to just rub the floor without moving the robot.

With my sketch, this gait would be something like:

Leg *GaitOrder[4] = {LegFL, LegBR, LegFR, LegBL};

  for (int leg=0; leg<4; leg++)
    for (int i = 0; i < 256; i++)
    {
      GaitOrder[leg]->move(i);
      delay(StepRateDelay);
    }
  }
11

Hi brother!! where i should put this code?

i already try to put this
within leg class, void setup even within void loop but there is error said

'Leg' to 'Leg*' in initialization

by the way this the link of the gait i mention

thank you for your respons

12

Where you want to take a step.

You have to put it somewhere after the four legs are declared.

It looks like all four legs are moving at the same time, just 90° out of phase instead of two pairs 180° out of phase. That would be a variation on the original gate, 64 steps apart instead of 128 steps apart.

13

i already put this part in void loop
and try to put this part of code

Leg *GaitOrder[4] = {LegFL, LegBR, LegFR, LegBL};

within all possible places in leg class also after this code

// Assign T, F, and C servo pins to each leg
Leg LegFR(39, 38, 37);
Leg LegBR(42, 41, 40);
Leg LegFL(31, 32, 33);
Leg LegBL(28, 29, 30);

but the error stil exist

14

Sorry, I forgot the '&' (AddressOf) operator to turn the Leg objects into Leg pointers.

This replaces the loop() in my sketch and compiles without error or warning. I don't have the hardware so I can't be sure it works.

Leg *GaitOrder[4] = {&LegFL, &LegBR, &LegFR, &LegBL};
void loop()
{
  for (int leg = 0; leg < 4; leg++)
    for (int i = 0; i < 256; i++)
    {
      GaitOrder[leg]->move(i);
      delay(StepRateDelay);
    }
}
16

hi brother
this code meet the forward move gait like the video

#include <Servo.h>
Servo sc1;  // create servo object to control a servo
Servo sf1;
Servo st1;
Servo sc2;  
Servo sf2;
Servo st2;
Servo sc3;  
Servo sf3;
Servo st3;
Servo sc4;  
Servo sf4;
Servo st4;

int i1L1 = 0;
int i2L1 = 0;
int i3L1 = 0;
int i4L1 = 0;

int i1L2 = 0;
int i2L2 = 0;
int i3L2 = 0;
int i4L2 = 0;

int i1L3 = 0;
int i2L3 = 0;
int i3L3 = 0;
int i4L3 = 0;

int i1L4 = 0;
int i2L4 = 0;
int i3L4 = 0;
int i4L4 = 0;


boolean l1status = LOW;
boolean l2status = LOW;
boolean l3status = LOW;
boolean l4status = LOW;

int speedV = 30;

void setup() {

 Serial.begin(9600);

sc1.attach(37);  
sf1.attach(38);
st1.attach(39);
sc2.attach(40);
sf2.attach(41);
st2.attach(42);
sc3.attach(33);
sf3.attach(32);
st3.attach(31);
sc4.attach(30);
sf4.attach(29);
st4.attach(28);

sc1.write(70);
sf1.write(85);
st1.write(94);//tibia 1
sc2.write(70);
sf2.write(90);
st2.write(90); // tibia 2
sc3.write(90);
sf3.write(95);
st3.write(90);// tibia 3
sc4.write(100);
sf4.write(86);//
st4.write(82);///tibia 4

  delay(4000);
}
void loop() {
  
   moveLeg1();
   if(l4status == HIGH) {
      moveLeg4();
   }
   if(l3status == HIGH) {
      moveLeg3();
  }
   if(l1status == HIGH){
      moveLeg2();
  }

    delay(speedV);
}

void moveLeg1() { 
  //rise the tibia
  if (i1L1 <= 10) {
    st1.write(98 - i1L1 * 2);
    sf1.write(90 - i1L1 * 3);
    i1L1++;
  }
  // rotate coax forward
  if (i2L1 <= 30) {
    sc1.write(70 + i2L1);
    i2L1++;
    if (i2L1 == 15){
    l4status = HIGH;
    }
  }
  // step down the tibia
  if (i2L1 > 20 & i3L1 <= 10) {
    st1.write(87 + i3L1 * 2);
    sf1.write(60 + i3L1 * 3);
    i3L1++;
  }
  // rotate coax back
  if (i2L1 >= 30) {
    sc1.write(100 - i4L1);
    i4L1++;
    l1status = HIGH;
  }
  if (i4L1 >= 30) {
    i1L1 = 0;
    i2L1 = 0;
    i3L1 = 0;
    i4L1 = 0;
  }
  }

void moveLeg2() {
  
   if (i1L2 <= 10) {
    st2.write(90 - i1L2 * 2);
    sf2.write(95 - i1L2 * 3);
    i1L2++;
  }
  
  if (i2L2 <= 30) {
    sc2.write(70 + i2L2);
    i2L2++;
  }
  
  if (i2L2 > 20 & i3L2 <= 10) {
    st2.write(70 + i3L2 * 2);
    sf2.write(60 + i3L2 * 3);
    i3L2++;
  }

  if (i2L2 >= 30) {
    sc2.write(100 - i4L2);
    i4L2++;
  }
  
  if (i4L2 >= 30) {
    i1L2 = 0;
    i2L2 = 0;
    i3L2 = 0;
    i4L2 = 0;
  }
}

void moveLeg3() { 
  
 if (i1L3 <= 10) {
    st3.write(83 + i1L3 * 2);
    sf3.write(90 + i1L3 * 3);
    i1L3++;
  }
  
  if (i2L3 <= 30) {
    sc3.write(100 - i2L3);
   i2L3++;
           if (i2L3 == 15){
    l2status = HIGH;
    }
  }
  
  if (i2L3 > 20 & i3L3 <= 10) {
    st3.write(90 - i3L3 * 2);
    sf3.write(110 - i3L3 * 3);
     i3L3++;
  }
  
  if (i2L3 >= 30) {
    sc3.write(70 + i4L3);
    i4L3++;
  
  }
  
  if (i4L3 >= 30) {
    i1L3 = 0;
    i2L3 = 0;
    i3L3 = 0;
    i4L3 = 0;
  }
  }

void moveLeg4() {
  
    if (i1L4 <= 10) {
    st4.write(70 + i1L4 * 2);
    sf4.write(75 + i1L4 * 3);
    i1L4++;
  }
  
  if (i2L4 <= 30) {
    sc4.write(100 - i2L4);
   i2L4++;
  }
  
  if (i2L4 > 20 & i3L4 <= 10) {
    st4.write(75 - i3L4 * 2);
    sf4.write(95 - i3L4 * 3);
     i3L4++;
  }
  
  if (i2L4 >= 30) {
    sc4.write(70 + i4L4);
    i4L4++;
    l3status = HIGH;
  }
  if (i4L4 >= 30) {
    i1L4 = 0;
    i2L4 = 0;
    i3L4 = 0;
    i4L4 = 0;
  }
}

but not as smooth as yours, would you like to help me to adapt the code as your code style.btw the code i modified just for forward move. thank you

17

Looks like the "status" variables are used to delay the activation of the various legs to get the 90° phase shift.

To do that with the Leg object it makes sense to add some logic to the Leg so it can delay starting until a specified offset in the gait. I'm going to work on that and report back.

18

Added some behaviors to the Leg object to allow phase differences between legs and forward or backward motion. This allows turning in place. See the loop() function for switching gaits and turning.

#include <Servo.h>

class Leg
{
  public:
    Leg(const byte TPin, const byte FPin, const byte CPin) : TPin(TPin), FPin(FPin), CPin(CPin)
    {
      latestPosition = 0;
      newPhaseOffset(0);
      forward();
    }

    void begin()
    {
      TServo.attach(TPin);
      FServo.attach(FPin);
      CServo.attach(CPin);
    }

    void newPhaseOffset(byte o)
    {
      waitingForPositionMatch = true;
      phaseOffset = o;
    }

    void forward()
    {
      if (!forwardMotion)
      {
        forwardMotion = true;
        waitingForPositionMatch = true;
      }
    }

    void backward()
    {
      if (forwardMotion)
      {
        forwardMotion = false;
        waitingForPositionMatch = true;
      }
    }

    void setTPositions(int Up, int Down)
    {
      TUpPosition = Up; 
      TDownPosition = Down; 
      TChange = TUpPosition - TDownPosition;
      TServo.write(TDownPosition);
    }

    void setFPositions(int Up, int Down)
    {
      FUpPosition = Up; 
      FDownPosition = Down; 
      FChange = FUpPosition - FDownPosition;
      FServo.write(FDownPosition);
    }

    void setCPositions(int Back, int Front)
    {
      CBackPosition = Back; 
      CFrontPosition = Front; 
      CChange = CFrontPosition - CBackPosition;
      CServo.write(CBackPosition);
    }

    void move(byte portion);  // 0-255 for each step.

  private:
    bool waitingForPositionMatch;  // Dont move until we reach the phaseOffset
    byte phaseOffset;  // Steps of phaseOffset between this leg and the gait position
    bool forwardMotion; // Direction forward
    byte latestPosition; 

    const byte TPin, FPin, CPin;
    Servo TServo, FServo, CServo;

    int TDownPosition, TUpPosition, TChange;
    int FDownPosition, FUpPosition, FChange;
    int CBackPosition, CFrontPosition, CChange;
};


// Pass a value from 0 to 255 to position the leg
// within a stride
void Leg::move(byte portion)
{
  // First adjust for the current phase phaseOffset
  portion -= phaseOffset;

  // Then apply the direction of motion
  if (!forwardMotion)
  {
    portion = 255 - portion;
  }

  // Are we waiting to hit phaseOffset?
  if (waitingForPositionMatch)
  {
    if (portion == latestPosition)  // Hit out phase point
      waitingForPositionMatch = false;
    else
      return;
  }

  latestPosition = portion;

  // 0-42: Lift the foot
  if (portion <= 42)
  {
    TServo.write(TDownPosition + TChange * portion / 42);
    FServo.write(FDownPosition + FChange * portion / 42);
  }

  // 0-128 steps: Rotate the leg forward.
  if (portion <= 128)
  {
    CServo.write(CBackPosition + CChange * portion / 128);
  }

  // 128 through 170 (42 steps): Put down the foot
  if (portion > 128 && portion < 170)
  {
    TServo.write(TUpPosition - TChange * (portion - 128) / 42);
    FServo.write(FUpPosition - FChange * (portion - 128) / 42);
  }

  // 128 through 255: Rotate the leg back
  if (portion > 128)
  {
    CServo.write(CFrontPosition - CChange * (portion - 128) / 128);
  }
}

const unsigned long MillisecondsPerStep = 1000;  // One step per second.
const unsigned long StepRateDelay = MillisecondsPerStep / 256;

// Assign T, F, and C servo pins to each leg
Leg LegFR(39, 38, 37);
Leg LegBR(42, 41, 40);
Leg LegFL(31, 32, 33);
Leg LegBL(28, 29, 30);

void setup()
{
  LegFR.setTPositions(90, 70);
  LegFR.setFPositions(90, 60);
  LegFR.setCPositions(70, 100);
  LegFR.begin();


  LegBR.setTPositions(90, 70);
  LegBR.setFPositions(90, 60);
  LegBR.setCPositions(70, 100);
  LegBR.begin();


  LegFL.setTPositions(90, 110);
  LegFL.setFPositions(90, 120);
  LegFL.setCPositions(70, 100);
  LegFL.begin();


  LegBL.setTPositions(90, 110);
  LegBL.setFPositions(90, 120);
  LegBL.setCPositions(70, 100);
  LegBL.begin();

  // Stand still for a few seconds
  delay(3000);
}

void gait180()
{
  LegFR.newPhaseOffset(0);
  LegBL.newPhaseOffset(0);
  LegFL.newPhaseOffset(128);
  LegBR.newPhaseOffset(128);
}

void gait90()
{
  LegFR.newPhaseOffset(0);
  LegBL.newPhaseOffset(64);
  LegFL.newPhaseOffset(128);
  LegBR.newPhaseOffset(192);
}

void forward()
{
  LegFL.forward();
  LegBL.forward();
  LegFR.forward();
  LegBR.forward();
}

void backward()
{
  LegFL.backward();
  LegBL.backward();
  LegFR.backward();
  LegBR.backward();
}

void turnLeft()
{
  LegFL.backward();
  LegBL.backward();
  LegFR.forward();
  LegBR.forward();
}

void turnRight()
{
  LegFL.forward();
  LegBL.forward();
  LegFR.backward();
  LegBR.backward();
}

void takeAStep()
{
  for (int i = 0; i < 256; i++)
  {
    LegFL.move(i);
    LegFR.move(i);
    LegBL.move(i);
    LegBR.move(i);
    delay(StepRateDelay);
  }
}

void loop()
{
  forward();
  gait90();

  // Take 5 steps
  for (int i = 0; i < 5; i++)
    takeAStep();

  gait180();

  // Take 5 steps
  for (int i = 0; i < 5; i++)
    takeAStep();


  // Turn left 5 steps
  turnLeft();
  for (int i = 0; i < 5; i++)
    takeAStep();

  // Pause
  delay(2000);
}
19

thank you brother it is work.

20

Hi brother john! if you don't mind i want to know where those numbers come from and the reason for it

 // 128 through 170 (42 steps): Put down the foot

and why you devide StepRateDelay with 256

thank you.

21

I used a byte (0-255) so you could increment it forever to move multiple steps forward and decrement it forever to move multiple steps backward. The byte will wrap from 255 to 0 when incrementing and 0 to 255 when decrementing.

The other numbers were estimates of what each joint was doing during each part of the cycle. They were modeled on your original code but using 256 steps instead of 50-ish.