arduino环境下利用ESP32控制舵狗（前进步态的编写）

舵狗的步态有许多种 ，我用的是比较常用的对角步态，及对角的足的动作一致不断交替运动完成行走的一种步态。
代码的实现方面是利用运动学的逆解计算出各角度值，然后与初始值进行比较，通过比较后的结果对舵机进行控制，就完成了舵狗的运动控制，其中需要注意的是延迟函数的使用，如果延迟过小会造成舵机来不及反应而只维持一个动作，如果延迟过大，会造成动作的不稳定。

#include <Arduino.h>
#include <WiFi.h>

#define DJ0_PWM 13
#define DJ1_PWM 12
#define DJ2_PWM 14

#define DJ3_PWM 27
#define DJ4_PWM 25
#define DJ5_PWM 26

#define DJ6_PWM 33
#define DJ7_PWM 32
#define DJ8_PWM 21

#define DJ9_PWM 19
#define DJ10_PWM 22
#define DJ11_PWM 23

double x,z,BD,xB,zB,L1 = 57,L2 = 50,L3 = 61;

double duty0,duty1,duty2,duty3,duty4,duty5,duty6,duty7,duty8,duty9,duty10,duty11;  // 占空比变量

double dutyCycle0,dutyCycle1,dutyCycle2,dutyCycle3,dutyCycle4,dutyCycle5,dutyCycle6,dutyCycle7,dutyCycle8,dutyCycle9,dutyCycle10,dutyCycle11;  // 设置占空比

double Initial_Alpha0,Initial_Beta0,
       Initial_Alpha1,Initial_Beta1,
       Initial_Alpha2,Initial_Beta2,
       Initial_Alpha3,Initial_Beta3;  //设置角度

int freq = 50;

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 freq3 = freq;    // 频率
int channel3 = 3;    // 通道0，共16个通道，0~15
int resolution3 = 10;   // 分辨率，取值0~20，duty最大取值为2^resolution-1

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

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

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

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

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

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

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

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

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

void Inverse_kinematics(double x,double z)  //逆运动学计算公式
{
   
  xB = 0;
  zB = 1.732/2*L1;
  if(x<0)
  {
   
    x = -x;
    BD = sqrt(x*x+(z-zB)*(z-zB));
    h.Alpha = 60+acos((x*x+BD*BD-(z-zB)*(z-zB))/(2*x*BD)