Arduino — 3DOF обратная кинематика ног Переполнение стека

Я пытаюсь реализовать обратную кинематику на квадрупеде с управлением Arduino, но я получил некоторые неточные результаты расчетов. Чтобы проверить, что не так, я перенес алгоритм на C ++ и запустил его на ПК. Моя проблема в том, что я предполагаю, что изменения координат дадут одинаковые результаты, независимо от того, в каком направлении я изменяю координаты. Но переходы по осям x и y дают разницу в несколько градусов, в зависимости от направления. Мои знания по математике немного запылены, поэтому, возможно, я что-то пропустил при проверке алгоритма. Ноги настроены так http://te.unib.ac.id/lecturer/indraagustian/wp-content/uploads/2014/05/Inverse-Kinematics.jpg

Что может вызвать разницу в результатах?

#include <cmath>
#include <iostream>

#define CONNECTED_SERVOS 12
#define PI 3.1415

#define coxa 55
#define femur 40
#define tibia 47

int nogo_buffer[CONNECTED_SERVOS];
int end_position[CONNECTED_SERVOS];
int cur_pw[CONNECTED_SERVOS];

int calibrate[] = { 1500, 1520, 1500, 1560, 1400, 1410, 1520, 1550, 1500, 1560, 1510, 1620 }; //calibrate servos to simmetrical positions

int floatToInt(double num) {
//Rounding a number avoiding truncation:

return (int)(num < 0 ? (num - 0.5) : (num + 0.5));
}

int radToMicro(double rad, int ref) {

//Make sure rad isn't negative:if (rad < 0) {
while (rad < -PI) {
rad += PI;
}
}
else {
while (rad > PI) {
rad -= PI;
}
}

//edit 2400
return ref - floatToInt(572.958 * rad);
}void IkLeg(int x, int y, int z, int nLeg)
{

double L1 = sqrt(pow(x,2) + pow(z,2));
double L = sqrt(pow(L1 - coxa,2) + pow(y,2));
double t = acos((pow(tibia,2) + pow(femur,2) - pow(L,2)) / (2 * tibia * femur)) / PI * 180;
double f1 = acos(y / L) / PI * 180;
double f2 = acos((pow(femur,2) + pow(L,2) - pow(tibia,2)) / (2 * femur * L)) / PI * 180;
double f = f1 + f2;
double c = atan2(z, x) / PI * 180;
std::cout << c << std::endl;
std::cout << f-90 << std::endl;
std::cout << t-90 << std::endl;

//coxa angle
nogo_buffer[3 * nLeg - 1] = radToMicro(c/180*PI, calibrate[3 * nLeg - 1]);

std::cout << nogo_buffer[3 * nLeg - 1] << std::endl;

//femur angle
nogo_buffer[3 * nLeg - 2] = radToMicro(f / 180 * PI, calibrate[3 * nLeg - 2]+900);

std::cout << nogo_buffer[3 * nLeg - 2] << std::endl;

//tibia angle
nogo_buffer[3 * nLeg - 3] = radToMicro(t / 180 * PI, calibrate[3 * nLeg - 3]+900);

std::cout << nogo_buffer[3 * nLeg - 3] << std::endl;
}

int main() {
IkLeg(95,47,0,1);
std::cout << "x transition" << std::endl;
IkLeg(105, 47, 0, 1);
IkLeg(85, 47, 0, 1);
std::cout << "y transition" << std::endl;
IkLeg(95, 57, 0, 1);
IkLeg(95, 37, 0, 1);
std::cout << "z transition" << std::endl;
IkLeg(95, 47, 30, 1);
IkLeg(95, 47, -30, 1);

return 0;
}