# Hexapod IK problem

2016-04-09_00.52.04.jpg (1329828Bytes)
2016-04-09_00.52.30.jpg (1317452Bytes)
2016-04-09_00.51.46.jpg (1211272Bytes)

Hi everyone, So I'm trying my hand at making a hexapod and have hit a snag which has me stuck now for a while. Basically I have built the hexapod and started coding it. Now I am trying to do body translation, i.e. the body moves but the feet stay perfectly planted, which works perfectly if I move the body up or down( my z direction). However when I move in the x or y direction, the feet do not stay planted at all. This can be seen in the 2nd and 3rd pic attached where the 1st pic is the starting position.

Now in excel the formulas work out fine, but in practice I have some problems, such as giving the coxa angle a addition of 500 instead of 1500. I'm using a mbed1768 and a ssc-32 to control the servos. Any help would be appreciated cause I'm stumped and have been for a while now.

#include "mbed.h"

#define LengthTibia 125         // Constants
#define LengthFemur 85
#define LengthCoxa 27

Serial ssc(p9, p10);  // tx, rx

DigitalIn Button1(p21);      //Assigning digital inputs
DigitalIn Button2(p22);
DigitalIn Button3(p23);
DigitalIn Button4(p24);

// Tibia - 0,4,8,16,20,24
// Femur - 1,5,9,17,21,25
// Coxa  - 2,6,10,18,22,26

void IK(float height, const float arrayx[], const float arrayy[], float AngleCoxa[], float AngleFemur[], float AngleTibia[]);

int main()
{
//leg       0       1     2     3      4     5
float arrayX [6] = {-79.2, -112.0,-79.2, 79.2, 112.0, 79.2};
float arrayY [6] = { 79.2,    0.0,-79.2,-79.2,   0.0, 79.2};
float angletibia [6];
float anglefemur [6];
float anglecoxa [6];
float Height = 125.0;
int P=0;

//VoltageCheck_tick.attach(&LowBatteryShutDown,0.5);  //start ticker
ssc.baud(115200);                                     //Set baud rate
ssc.printf("#1 PO50 #5 PO-20 #6 PO-40 #9 PO60 #17 PO-20 #21 PO-30 #22 PO-30 #25 PO30 #26 PO-100 \r");

IK(Height, arrayX, arrayY, anglecoxa ,anglefemur ,angletibia);
for(int i = 0; i<6 ;i++)
{
ssc.printf("#%d P%.0f #%d P%.0f #%d P%.0f \r",P,angletibia[i],P+1,anglefemur[i],P+2,anglecoxa[i]);   //send new co-ordinates
P+=4;               //next leg
if(P==12)   {P=16;}
if(P==28)   {P= 0;}
}

while(1)
{
if (Button1==1)
{
wait(0.5);
while(Button1==0)
{
if (Button3==1)  {     Height = Height+1;    }
if (Button4==1)  {     Height = Height-1;    }

if(Joystick1<=0.25)
{
for(int i=0;i<6;i++)    {arrayY[i] += -0.5;}
}
else if(Joystick1>=0.75)
{
for(int i=0;i<6;i++)    {arrayY[i] +=  0.5;}
}

if(Joystick2<=0.25)
{
for(int i=0;i<6;i++)    {arrayX[i] +=  0.5;}
}
else if(Joystick2>=0.75)
{
for(int i=0;i<6;i++)    {arrayX[i] += -0.5;}
}
IK(Height, arrayX, arrayY, anglecoxa ,anglefemur ,angletibia);
for(int i = 0; i<6 ;i++)
{
ssc.printf("#%d P%.0f #%d P%.0f #%d P%.0f \r",P,angletibia[i],P+1,anglefemur[i],P+2,anglecoxa[i]);   //send new co-ordinates
P+=4;               //next leg
if(P==12)   {P=16;}       //change to legs on right side
if(P==28)   {P= 0;}
}
}
wait(0.5);

}
}
}

void IK(float height, const float arrayx[], const float arrayy[], float AngleCoxa[], float AngleFemur[], float AngleTibia[])
{
float LegLength, Hyp, x, y, deg = 5.497787;  //315deg

for(int i = 0; i<6 ;i++)
{
x = (arrayx[i]*cos(deg)-arrayy[i]*sin(deg));
y = (arrayx[i]*sin(deg)+arrayy[i]*cos(deg));

LegLength = sqrt((x*x)+(y*y));
Hyp = sqrt((height*height)+((LegLength-LengthCoxa)*(LegLength-LengthCoxa)));

AngleCoxa[i] = 1.570796-(atan2(x,y));

AngleFemur[i] = ((atan2((LegLength-LengthCoxa),height))+(acos(((LengthTibia*LengthTibia)-(LengthFemur*LengthFemur)-(Hyp*Hyp))/(-2*LengthFemur*Hyp))))-1.570796;

AngleTibia[i] = 1.570796-(acos(((Hyp*Hyp)-(LengthTibia*LengthTibia)-(LengthFemur*LengthFemur))/(-2*LengthTibia*LengthFemur)));

if(i==2)  {deg += -1.570796;}  // minus 90deg
else {deg += -0.785398;}       // minus 45deg
//adjust angles for ssc-32 range (500 to 2500)
if(i<=2)
{
AngleCoxa[i] = AngleCoxa[i]*636.7+500;
AngleFemur[i] = AngleFemur[i]*636.7+1500;
AngleTibia[i] = AngleTibia[i]*636.7+1500;
}
else
{
AngleCoxa[i] = AngleCoxa[i]*636.7+500;
AngleFemur[i] = 1500-AngleFemur[i]*636.7;
AngleTibia[i] = 1500-AngleTibia[i]*636.7;
}
}
}

IK Calculation

Why is leg 2 being singled out in the IK calculation. Should

if(i==2)  {deg += -1.570796;}  // minus 45deg
else {deg += -0.785398;}

be

if (i <= 2)  {deg += -1.570796;}  // minus 45deg
else {deg += -0.785398;}

no thats fine.The legs are

no thats fine.

The legs are not positioned equally around the hexapod at 60deg.

leg 5 is 45deg from the front,
leg 4 is 90deg
leg 3 is 135deg
leg 2 is 225deg
leg 1 is 270deg
leg 0 is 315deg

where there is a difference of 45deg between each preceding leg except between leg 3 and 2 which is 90deg. Had a mistake in the comments, fixed it now.