Hexapod servo motors twitching [Solved]

Hexanoob · April 26, 2020, 4:47pm

Solved it with the proper use of else if..................................................................

Because I didn't put "else if" the twitching servos got sent pwm signals for each leg

sorry for wasting your time people

Hi,

I have programmed a hexapod, which have 6 legs and 18 servo motors. To control all the motors I have connected a PCA9865 pwm board to my Arduino Uno. 16 motors from pwm board, 2 from Arduino. All of the motors from the pwm board is working as they should, but the 2 pwm outsputs from the arduino are making the motors twitch. I suspect the problem might be because of all the traffic between the arduino and pwm board.

The "twitchy" leg works perfectly when I run it alone, but starts twitching when running with the other legs.

Do you guys know what the problem might be? And a work around for it?

Any help or general feedback on the code (added as an attachment to the post) would be highly apriciated

With all legs

one leg

Video of the original creator of the hexapod. Using the same parts as me, but using LINKIT (JS) instead of Arduino

Jumbo.ino (5.89 KB)

Delta_G · April 26, 2020, 4:53pm

OP's code where someone can actually see it and try to help him or her.

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <Servo.h>
#include <math.h> 


Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);

#define MIN_PULSE_WIDTH       1000
#define MAX_PULSE_WIDTH       2000
#define DEFAULT_PULSE_WIDTH   1500
#define FREQUENCY             50
#define   M_PI   3.14159265358979323846 /* pi */
#define   atanf   atan

Servo myservo16;  // create servo object to control a servo
Servo myservo17;  // create servo object to control a servo
  int angle16 = 0;
  int angle17 = 0;
  int i=0, j=0, k=0;
  int leg=0;
  int loc0=0, loc1=0, loc2=0;
  int a = 42, b = 84, c = 36;
  double x=0, y=0, z=0, xp=0, yp=0, lr=0, ar=0, at=0, az=0;
  int a0=0, a1=0, a2=0;


//The nullpunkt for each servo in ms
int ms[6][3]={
  {1550, 1550, 1450}, 
  {1550, 1600, 1450}, 
  {1600, 1450, 1400}, 
  {1550, 1500, 1400}, 
  {1600, 1450, 1500}, 
  {1500, 1500, 1500}
  };

// x, y, z coordinates of nodes of each leg 
int legCenters[6][3] = {
  {40.36, 70.36, 0},
  {55.25, 0, 0},
  {40.36, -70.36, 0},
  {-40.36, -70.36, 0},
  {-55.25, 0, 0},
  {-40.36, 70.36, 0},
};

int legAngles[6] = {45, 0, 315, 225, 180, 135};


// default x, y, z coordinates of tips of each leg 
int legTipsDefault[6][3] = {
  {95.52,   125.52,   -62},
  {133.25,  0,        -62},
  {95.52,   -125.52,  -62},
  {-95.52,  -125.52,  -62},
  {-133.25, 0,        -62},
  {-95.52,  125.52,   -62}
  };

// "current" x, y, z coordinates of tips of each leg 
int legTipsLoc[6][3] = {
  {0, 0, 0},
  {0, 0, 0},
  {0, 0, 0},
  {0, 0, 0},
  {0, 0, 0},
  {0, 0, 0}
  };

int fwdTable[40][3] = {
  {0, -40, 0},  
  {8, -38, 12},  
  {24, -32, 24},  
  {32, -24, 32},  
  {38, -12, 38},
  {40, 0, 40},  
  {38, 12, 38},  
  {32, 24, 32},  
  {24, 32, 24},  
  {12, 38, 12},
  {0, 40, 0},  
  {0, 32, 0},  
  {0, 24, 0},  
  {0, 16, 0},  
  {0, 8, 0},
  {0, 0, 0},  
  {0, -8, 0},  
  {0, -16, 0},  
  {0, -24, 0},  
  {0, -32, 0},
  {0, -40, 0},  
  {12, -38, 12},  
  {24, -32, 24},  
  {32, -24, 32},  
  {38, -12, 38},
  {40, 0, 40},  
  {38, 12, 38},  
  {32, 24, 32},  
  {24, 32, 24},  
  {12, 38, 12},
  {0, 40, 0},  
  {0, 32, 0},  
  {0, 24, 0},  
  {0, 16, 0},  
  {0, 8, 0},
  {0, 0, 0},  
  {0, -8, 0},  
  {0, -16, 0},  
  {0, -24, 0},  
  {0, -32, 0}
};




// return angles 0, 1, 2 of specified "leg" for its tip to reach specified "location"
// return null if impossible
long loc2angles(int leg, int loc0,int loc1,int loc2)
{
//      Serial.print("\n \n \n \n");
if (leg >= 3) {
  loc0=loc0*-1;
  loc1=loc1*-1;
 // loc2=loc2*-1;  
}
  
  //Serial.print("leg="),Serial.println(leg);
  //Serial.print("loc0="),Serial.println(loc0);
  //Serial.print("loc1="),Serial.println(loc1);
  //Serial.print("loc2="),Serial.println(loc2);
  
  // shift origin to leg center
  x = loc0 - legCenters[leg][0]+legTipsDefault[i][0];
  //Serial.print("x="),Serial.println(x);
  y = loc1 - legCenters[leg][1]+legTipsDefault[i][1];
  //Serial.print("y="),Serial.println(y);
  z = loc2 - legCenters[leg][2]+legTipsDefault[i][2];
  //Serial.print("z="),Serial.println(z);
  
  a0 = (180/M_PI)* atan(y/x) - legAngles[leg];
  if(x<0)
    a0 += 180;
  else if(y<0)
    a0 += 360;
  if(a0 > 180)
    a0 -= 360;

  xp = sqrt(x*x+y*y)-c;
  yp = z;

  lr = sqrt(xp*xp+yp*yp);
  ar = (180/M_PI)*asin(yp/lr);
  at = (180/M_PI)*acos((lr*lr + a*a - b*b)/(2*a*lr));
  az = (180/M_PI)*acos((lr*lr - a*a + b*b)/(2*b*lr));
  a1 = at + ar;
  a2 = 180 - at - az;

  //Serial.print("a0="),Serial.println(a0);
  //Serial.print("a1="),Serial.println(a1);
  //Serial.print("a2="),Serial.println(a2);
  
  return a0, a1, a2;

}

// nonblockingly move tip of specified "leg" to specified "location"
// return true if ok
// return false if impossible
long moveLeg(int leg,int loc0,int loc1,int loc2)
{
  a = loc2angles(leg, loc0, loc1, loc2);

  if(a0 <= 45 && a0 >= -45 &&
      a1 <= 90 && a1 >= -90 &&
      a2 <= 150 && a2 >= 36)
  {


       
     if (leg<=2) { 
      pwm.writeMicroseconds(3*leg, ms[leg][0] + a0 * 500 / 45);
      pwm.writeMicroseconds(3*leg+1, ms[leg][1] - a1 * 500 / 45);
      pwm.writeMicroseconds(3*leg+2, ms[leg][2] + (a2-90) * 500 / 45);
    }
    if (leg>=3 && leg<=4) {
      pwm.writeMicroseconds(3*leg, ms[leg][0] - a0 * 500 / 45);
      pwm.writeMicroseconds(3*leg+1, ms[leg][1] + a1 * 500 / 45);
      pwm.writeMicroseconds(3*leg+2, ms[leg][2] - (a2-90) * 500 / 45);
    } 
    else  {
      pwm.writeMicroseconds(15, ms[5][0] - a0 * 500 / 45);
      myservo16.writeMicroseconds(ms[5][1] + a1 * 500 / 45);    
      myservo17.writeMicroseconds(ms[5][2] - (a2-90) * 500 / 45);  
      }
    //Serial.print("leg="),Serial.println(leg);
    //Serial.print("a0="),Serial.println(a0); 
    //Serial.print("ms0="),Serial.println(ms[leg][0] + a0 * 500 / 45 );
    //Serial.print("motor="),Serial.println(1*leg); 
    //Serial.print("a1="),Serial.println(a1);
    //Serial.print("ms1="),Serial.println(ms[leg][1] - a1 * 500 / 45 );
    //Serial.print("a2="),Serial.println(-a2);
    //Serial.print("ms2="),Serial.println(ms[leg][2] + (a2-90) * 500 / 45);
  }
}
void setup () {
  pwm.begin();
  pwm.setPWMFreq(FREQUENCY);
  delay(2000); 
  //Serial.begin(9600);
  myservo16.attach(8);  // attaches the servo on pin 9 to the servo object
  myservo17.attach(9);  // attaches the servo on pin 9 to the servo object
}

void loop() {
  for (int k = 0; k<=100 ; k++) {
for (j = 0; j<=19  ; j++) {
  for (i = 0; i <= 5; i++) {
    if ((i % 2)==0) {
    loc0=fwdTable[j][0]; 
    loc1=fwdTable[j][1];
    loc2=fwdTable[j][2];
    moveLeg(i,loc0, loc1, loc2); 
    }
    else {
    loc0=fwdTable[j+10][0]; 
    loc1=fwdTable[j+10][1];
    loc2=fwdTable[j+10][2];
    moveLeg(i,loc0, loc1, loc2);
    }
    //delay(40);
    //Serial.print("\n \n \n \n");
  }
//  Serial.print("\n \n \n \n");
  }
  delay(50);
}
}

Delta_G · April 26, 2020, 4:55pm

Show us some wiring too. How are you powering all these servos, especially the two with issues.

Hexanoob · April 26, 2020, 5:14pm

Here is how I connected it

Delta_G · April 26, 2020, 5:17pm

I'd appreciate it if you'd attach the picture here. I won't see it if it is on another site.

Hexanoob · April 26, 2020, 5:25pm

Put the wrong link in the "insert a image", havn't been posting too much on this forum

The power should not be the problem, as it is getting power the same way as the rest of the legs which does not have this issue.

Its like the pwm signals from the arduino is somehow being distrupted by the communication between the arduino and the pwm board. No idea wether this is a code issue or hardware issue.

Hexanoob · April 26, 2020, 5:54pm

vinceherman:
Power is indeed a common problem.
Also, if any of those other libraries use the same timer as Servo.h, you might run in to problems.
A way to get around this is to move the servo signal management off the arduino and on to a driver board.
Adafruit has a 16 channel board for this.

Edit: Here is a link to the Adafruit board.

The two libraries might use the same timer?

Delta_G · April 26, 2020, 8:33pm

How does it communicate with the servo controller? What is that line? That blue wire? Is it Serial? I2C? SPI?

Hexanoob:
The two libraries might use the same timer?

You've got the library, I don't. What's up with that Adafruit_PWMServoDriver.h library. Where can I see that?

Hexanoob · April 26, 2020, 9:40pm

Thank you for looking into it:)

It is using I2C to communicate.

This is the library:

Which you can get in the arduino app:

jremington · April 27, 2020, 2:50pm

The "twitchy" leg works perfectly when I run it alone, but starts twitching when running with the other legs.

The power supply is clearly inadequate.

You should budget 1 Ampere per servo for even small hobby servos (up to 3 A/servo for powerful ones like MG996R).

So plan on a power supply capable of delivering 18 Amperes, absolute minimum. Can your battery do that?

Please post links to your DC-DC converters. I'll bet none of them can handle 6 Amperes.

Hexanoob · April 27, 2020, 5:18pm

I'm using a 20C 2,8Ah battery, whic means an output of 56A as far as I know. The voltage regulators can deliver 3A, and the SG90 servos use 550mA. I'm certain that it is not the problem

vinceherman · April 27, 2020, 5:47pm

Hexanoob:
The "twitchy" leg works perfectly when I run it alone, but starts twitching when running with the other legs.

How did you run these tests? Different code? Or the same code but power removed from the other servos?

If it is the same code, then I think we can say you are properly producing servo commands. The next most logical piece would be the power supply.

Do you have another battery and regulator? Wire it up to power just the servos that we twitching.

jremington · April 27, 2020, 7:26pm

The SG-90 stall current is 650+/-80 mA, and you plan to run six of them from one regulator, which you claim is capable of 3A.

I would wish you good luck with that attitude, but luck hasn't worked out for you.

Hexanoob · April 27, 2020, 7:50pm

vinceherman:
How did you run these tests? Different code? Or the same code but power removed from the other servos?

Same code in video, but with changes in loop for leg i=0 (all 6 legs), i=5(1 leg)

In the video with 1 leg, 2 of the servos are from the arduino uno pwm outputs and 1 leg from the pwm board

vinceherman:
Do you have another battery and regulator? Wire it up to power just the servos that we twitching.

I'm 100% sure its not a power issue, as all the 5 other legs are working as intended

jremington:
The SG-90 stall current is 650+/-80 mA, and you plan to run six of them from one regulator, which you claim is capable of 3A.

I would wish you good luck with that attitude, but luck hasn't worked out for you.

Hexapod - LinkIt Smart 7688 with 3D printed body - YouTube, this is the guy who created the original project. He is using 6 servos on each regulator as well. It's working because all 6 of the servos are not working at max load all the time

vinceherman · April 27, 2020, 8:30pm

Hexanoob:
I'm 100% sure its not a power issue, as all the 5 other legs are working as intended

I am glad that you are so confident.
I am not.

I have worked with servos and arduinos a fair bit.
I have coached many people on these forums with various issues.
Other responders on this thread that have much more experience than me also want to rule out power problems.

As I said, if the code looks good, the next most likely problem is power.

Most people would actually put it the other way round. Power is the number 1 problem when users have servo issues.

I could be wrong. I often am.
But my advice is on power.

Hexanoob · April 27, 2020, 10:14pm

Aaaaaaaaaaah, finally figured it out! It was a giant facepalm of a problem. Didn't use if, else if, else properly.................

vinceherman · April 28, 2020, 10:34pm

Glad that you got it working!

system · May 6, 2021, 11:44am