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);
AnalogIn Joystick1(p18); //Assigning analog inputs
AnalogIn Joystick2(p17);
AnalogIn Ain3(p15);
// 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;
}
}
}