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);
}
}
