c++ - c++ 2D伺服机器人手臂逆运动学

Question

我目前正在尝试编写代码来指示机械臂（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);
          }


    }
   `}

非常感谢任何对我出错的地方的帮助