im making a hexopod, and for now i decided to make only one leg, and controll it with coordinates, the measure for distance is milimeters, the measure for angles is degrees, however i have a hard time believing that this

```
turnToPos(81, 51, 0);
```

results in theese angles

beta = 61.82

-35

61

32

here is the entirety of the code:

```
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include<cmath>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define SERVOMIN 60
#define SERVOMAX 500
uint8_t servo = 0;
uint8_t servo1 = 1;
uint8_t servo2 = 2;
int legAngle1;
int legAngle;
int legAngle2;
int angle;
int legLength1 = 106;
int legLength2 = 104;
int distance;
double pi = M_PI;
void setup() {
Serial.begin(9600);
pwm.begin();
pwm.setPWMFreq(50);
}
void turnToAngle(int pin, int angle) {
float pulse;
pulse = map(angle, -90, 90, SERVOMIN, SERVOMAX);
pwm.setPin(pin, pulse);
Serial.println(angle);
}
void turnToPos(float x, float y, float z){
float l = sqrt(pow(x, 2) + pow(y, 2)); // length between x and y
float h = sqrt(pow(l, 2) + pow(z, 2)); // length between l and z
float beta = (180/pi)*acos((pow(legLength1, 2) + pow(h, 2) - pow(legLength2, 2))/(2 * legLength1 * h));
Serial.print("beta = ");
Serial.println(beta);
legAngle2 = (180/pi)*asin(y/l);
if(z == 0){
legAngle1 = beta;
}else if(z > 0){
legAngle1 = beta + (180/pi)*asin(z/h);
}else if(z < 0){
legAngle1 = (180/pi)*asin(z/h);
}
legAngle = 90 - (180 - ((180/pi)*acos((pow(legLength1, 2) + pow(legLength2, 2) - pow(h, 2))/(2 * legLength1 * legLength2))));
turnToAngle(servo, legAngle);
turnToAngle(servo1, legAngle1);
turnToAngle(servo2, legAngle2);
}
void loop() {
turnToPos(81, 51, 0);
Serial.println("pos1^");
delay(1000);
turnToPos(40, -30, -5);
Serial.println("pos2^");
delay(1000);
}
```

and even tho the leg length isnt exact i doubt that a difference in 2 mm will make that huge of a difference.

Leg angle is for the leg furthest from the body, leg angle 1 is for the middle segment and leg angle 2 is for rotating the leg horisontaly