Hi guys,
I’m trying to make my hexapod Phoenix 3DOF walk so I started to write code. I use Arduino Uno and Lynxmotion SSC-32U. I’m stuck on math calculations and I have difficulties to move on. So I found IK calculations online, there are different sites explaining the formulas and as I can understand they all are aligned.
I used one from Oscar Liang from Trigonometry Basic and I started from coxa joint. I choose left middle leg, and as starting point I decided coordinates (0,0,0) - (x,y,z). It’s the point where coxa is attached to the case.
Then I measured where this leg touchs the ground and it’s coordinate (0, 131, - 126) - z is negative because positive direction is up.
I want to make a calculations for coxa servo so when X moves forward and backward servo should change a possion accordingly.
When leg is going back, x is increasing and servo should move leg to the back, and when it goes forward then x is decreasing
So formula for this angle is
This is a code I wrote:
// coxa - 27mm
// femore - 82mm
// tibia - 122mm
// z-offset - 112mm
#include <SoftwareSerial.h>
float rootX = 0;
float rootY = 0;
float rootZ = 0;
float groundX = 131;
float groundY = 1;
float groundZ = 126;
SoftwareSerial usc(10,11);
void setup() {
Serial.begin(9600);
usc.begin(115200);
usc.println("#20P900");
delay(2000);
float zero = 1500;
float error = -250;
float servoMin = 900;
float servoMax = 1900;
float maxAngle = 90;
float gama = degrees(atan(groundX / groundY));
float servoPosition = zero + error + gama/maxAngle * (servoMax - servoMin);
for(int i = -131; i < 131; i +=5 ) {
delay(100);
groundY = i;
Serial.print("y = mm: ");
Serial.print(i);
Serial.print(" x/y = ");
Serial.print(groundX / groundY);
gama = degrees(atan(groundX / groundY));
Serial.print(" gama: ");
Serial.print(gama);
servoPosition = (zero + error) - (gama/maxAngle * (servoMax - servoMin));
Serial.print(" servo position: ");
Serial.println(servoPosition);
}
}
void loop() {
}Problem is that I’m trying to move leg from forward to back changing only X coordinate (from -131 to 131) but angles are changing from -45 to -90 and from 90 to 45.
This is the output I’m getting
y = mm: -131 x/y = -1.00 gama: -45.00 servo position: 1750.00
y = mm: -126 x/y = -1.04 gama: -46.11 servo position: 1762.38
y = mm: -121 x/y = -1.08 gama: -47.27 servo position: 1775.25
y = mm: -116 x/y = -1.13 gama: -48.48 servo position: 1788.61
y = mm: -111 x/y = -1.18 gama: -49.72 servo position: 1802.49
y = mm: -106 x/y = -1.24 gama: -51.02 servo position: 1816.91
y = mm: -101 x/y = -1.30 gama: -52.37 servo position: 1831.87
y = mm: -96 x/y = -1.36 gama: -53.77 servo position: 1847.39
y = mm: -91 x/y = -1.44 gama: -55.21 servo position: 1863.49
y = mm: -86 x/y = -1.52 gama: -56.72 servo position: 1880.17
y = mm: -81 x/y = -1.62 gama: -58.27 servo position: 1897.45
y = mm: -76 x/y = -1.72 gama: -59.88 servo position: 1915.33
y = mm: -71 x/y = -1.85 gama: -61.54 servo position: 1933.81
y = mm: -66 x/y = -1.98 gama: -63.26 servo position: 1952.89
y = mm: -61 x/y = -2.15 gama: -65.03 servo position: 1972.57
y = mm: -56 x/y = -2.34 gama: -66.85 servo position: 1992.82
y = mm: -51 x/y = -2.57 gama: -68.73 servo position: 2013.65
y = mm: -46 x/y = -2.85 gama: -70.65 servo position: 2035.02
y = mm: -41 x/y = -3.20 gama: -72.62 servo position: 2056.90
y = mm: -36 x/y = -3.64 gama: -74.63 servo position: 2079.27
y = mm: -31 x/y = -4.23 gama: -76.69 servo position: 2102.07
y = mm: -26 x/y = -5.04 gama: -78.77 servo position: 2125.27
y = mm: -21 x/y = -6.24 gama: -80.89 servo position: 2148.81
y = mm: -16 x/y = -8.19 gama: -83.04 servo position: 2172.63
y = mm: -11 x/y = -11.91 gama: -85.20 servo position: 2196.67
y = mm: -6 x/y = -21.83 gama: -87.38 servo position: 2220.86
y = mm: -1 x/y = -131.00 gama: -89.56 servo position: 2245.14
y = mm: 4 x/y = 32.75 gama: 88.25 servo position: 269.43
y = mm: 9 x/y = 14.56 gama: 86.07 servo position: 293.67
y = mm: 14 x/y = 9.36 gama: 83.90 servo position: 317.78
y = mm: 19 x/y = 6.89 gama: 81.75 servo position: 341.69
y = mm: 24 x/y = 5.46 gama: 79.62 servo position: 365.35
y = mm: 29 x/y = 4.52 gama: 77.52 servo position: 388.69
y = mm: 34 x/y = 3.85 gama: 75.45 servo position: 411.66
y = mm: 39 x/y = 3.36 gama: 73.42 servo position: 434.21
y = mm: 44 x/y = 2.98 gama: 71.43 servo position: 456.29
y = mm: 49 x/y = 2.67 gama: 69.49 servo position: 477.87
y = mm: 54 x/y = 2.43 gama: 67.60 servo position: 498.91
y = mm: 59 x/y = 2.22 gama: 65.75 servo position: 519.40
y = mm: 64 x/y = 2.05 gama: 63.96 servo position: 539.31
y = mm: 69 x/y = 1.90 gama: 62.22 servo position: 558.63
y = mm: 74 x/y = 1.77 gama: 60.54 servo position: 577.35
y = mm: 79 x/y = 1.66 gama: 58.91 servo position: 595.47
y = mm: 84 x/y = 1.56 gama: 57.33 servo position: 612.99
y = mm: 89 x/y = 1.47 gama: 55.81 servo position: 629.91
y = mm: 94 x/y = 1.39 gama: 54.34 servo position: 646.24
y = mm: 99 x/y = 1.32 gama: 52.92 servo position: 661.99
y = mm: 104 x/y = 1.26 gama: 51.55 servo position: 677.18
y = mm: 109 x/y = 1.20 gama: 50.24 servo position: 691.81
y = mm: 114 x/y = 1.15 gama: 48.97 servo position: 705.90
y = mm: 119 x/y = 1.10 gama: 47.75 servo position: 719.47
y = mm: 124 x/y = 1.06 gama: 46.57 servo position: 732.53
y = mm: 129 x/y = 1.02 gama: 45.44 servo position: 745.10I know I’m calculating something wrong but I can not understand what.
Thanks in advance.
p.s. If I posted in wrong section please move it in correct.