在arduino环境下用ESP32控制机械臂(逆运动学算法)

• 我用的机械臂的动力来源是舵机，所以需要通过pwm波来控制，而在arduino中没有专门输出pwm波的函数有的是一个ledc的函数，它是用来控制led灯的，但是同样可以用来输出pwm波。
• ledc详细的学习可以参考这位大佬的博客
• 我们首先需要确定机械臂的初始位置并找到各个舵机的初始值，这需要根据实际的数据去自己测定，这里我们以L2与Z轴重合方向指向Z轴正方向，L3垂直L2方向指向X正轴为初始位置且认为舵机此时的角度为0度(这里舵机的角度取值范围为-90°~90°)机械臂状态如下图：

正视图

俯视图

下面是程序实现

#include 

double x, y, z, AE, A, L2 = 10, L3 = 10;  //L2和L3输入机械臂各部分的长度,这里假设都为10

#define DJ0_PWM 13  //定义输出pwm波的IO口
#define DJ1_PWM 12
#define DJ2_PWM 14

int freq = 50;  //设置频率50Hz也就是周期20ms

int freq0 = freq;    // 频率
int channel0 = 0;    // 通道0，共16个通道，0~15
int resolution0 = 10;   // 分辨率，取值0~20，duty最大取值为2^resolution-1

int freq1 = freq;    // 频率
int channel1 = 1;    // 通道1，共16个通道，0~15
int resolution1 = 10;   // 分辨率，取值0~20，duty最大取值为2^resolution-1

int freq2 = freq;    // 频率
int channel2 = 2;    // 通道0，共16个通道，0~15
int resolution2 = 10;   // 分辨率，取值0~20，duty最大取值为2^resolution-1

int dutyCycle0;  // 设置占空比变量
int dutyCycle1;
int dutyCycle2;

struct ForwardKinematics  //建立一个结构体用来存储计算后的角度值
{
  double Alpha;  //角度α
  double Beta;  //角度β
  double Gamma;  //角度γ
}h;

void Inverse_kinematics(double x,double y,double z)  //逆运动学计算公式
{
  AE=x*x+y*y;
  A=atan(z/x)*180/PI+acos((L2*L2+z*z+(x*x+y*y)*(x*x+y*y)-L3*L3)/(2*L2*sqrt(z*z+(x*x+y*y)*(x*x+y*y)*(x*x+y*y)*(x*x+y*y))))*180/PI;
  h.Alpha=A;
  h.Beta=acos((L2*L2+L3*L3-z*z-(x*x+y*y)*(x*x+y*y)*(x*x+y*y)*(x*x+y*y))/(2*L2*L3))*180/PI;
  h.Gamma=atan(y/x)*180/PI;
}
void ledc_Initialization() // ledc初始化程序
{
  ledcSetup(channel0, freq0, resolution0); // 设置通道0
  ledcSetup(channel1, freq1, resolution1); // 设置通道1
  ledcSetup(channel2, freq1, resolution2); // 设置通道1

  ledcAttachPin(DJ0_PWM, channel0);  // 将通道0与引脚13连接
  ledcAttachPin(DJ1_PWM, channel1);  // 将通道1与引脚12连接
  ledcAttachPin(DJ2_PWM, channel2);  // 将通道1与引脚14连接
}



void setup()
{
  ledc_Initialization();  // ledc初始化程序
  Inverse_kinematics(10,0,0);  //在此处输入机械臂尖端的x,y,z坐标值
  dutyCycle0 = 511+h.Alpha*1024/180;
  dutyCycle1 = 511+h.Beta*1024/180;
  dutyCycle2 = 511+h.Gamma*1024/180;
}

void loop()
{
  ledcWrite(channel0,dutyCycle0);  // 输出PWM,dutyCycle为占空比
  ledcWrite(channel1,dutyCycle1);  // 输出PWM,dutyCycle为占空比
  ledcWrite(channel2,dutyCycle2);  // 输出PWM,dutyCycle为占空比
  delay(5);
}