C ++ 2D сервопривод манипулятора обратной кинематики

В настоящее время я пытаюсь написать код для инструктирования манипулятора робота (2 сервопривода, 3 джойстика) записывать слова, и у меня возникают проблемы с его правильной настройкой. Сейчас я просто пытаюсь заставить его перейти к вектору. Я думаю, что часть обратной кинематики верна, но все, что я получаю, — это дергание от одного из двигателей (если мне повезет). Используется плата Nucleo-F411RE, и я использую разработчик mbed.

#include "mbed.h"#include "Servo.h"#include "math.h"
Servo servo1(PA_8), servo2(PA_9);
float len1 = 9.0;
float len2 = 7.5;
double pi = 3.1415926535897;

int lawOfCosines (float a, float b, float c )
{
return acos((a*a + b*b - c*c) / (2 * a * b));
}
int distance(float x, float y) {
return sqrt(x*x + y*y);
}

int deg(float rad) {
return rad * 180 / pi;
}
int main() {

//fmt.Println("Lets do some tests. First move to (5,5):");
float x = 5.0;
float y = 5.0;
float dist = distance(x, y);
float D1 = atan2(y, x);
float D2 = lawOfCosines(dist, len1, len2);
float A1 = D1 + D2;
float A2 = lawOfCosines(len1, len2, dist);

float  m1 = A1 * (100/90);
float  m2 = A2 * (100/90);
for(int i=0; i<m1; i++) {
servo2 = i/100.0;

wait(0.01);
}
for(int i=0; i<m2; i++) {
servo1 = i/100.0;
wait(0.01);
}}
`}

Любая помощь, где я иду не так, с благодарностью