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:

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.Code:// 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() { }

This is the output I'm getting

I know I'm calculating something wrong but I can not understand what.Code: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.10

Thanks in advance.

p.s. If I posted in wrong section please move it in correct.

## Bookmarks