c ++ 2D伺服机器人手臂反向运动学

时间:2017-04-24 05:17:15

标签: c++ robot mbed inverse-kinematics

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


    }
   `}

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

0 个答案:

没有答案