arduino下esp32连接openmv

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

#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

#define DJ0_PWM 32
#define DJ1_PWM 33
#define DJ2_PWM 25

#define DJ3_PWM 26
#define DJ4_PWM 27
#define DJ5_PWM 14

#define DJ6_PWM 12
#define DJ7_PWM 13
#define DJ8_PWM 16

#define DJ9_PWM 17
#define DJ10_PWM 21
#define DJ11_PWM 22

//hw_timer_t* timer= NULL;  //定义存放定时器的指针

BluetoothSerial SerialBT;

char wordmode;    //模式变量   0为蓝牙，1为openmv
char comchar;     //串口接收变量
char BT;          //蓝牙接收变量

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

double d0 = 73.25,d3 = 76.8,d6 = 76.8,d9 = 69.7;

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)  //逆运动学计算公式
{
   
  double AC,L1 = 40,L2 = 40;
  if(x<0)
  {
   
    x = -x;
    AC = sqrt(x*x+z*z);
    h.Alpha = 180-acos((x*x+AC*AC-z*z)/(2*x*AC))*180/PI+acos((L1*L1+AC*AC-L2*L2)/(2*L1*AC))*180/PI;
    h.Beta = acos((L1*L1+L2*L2-AC*AC)/(2*L1*L2))*180/PI;
  }else if(x>0)
  {
   
    AC = sqrt(x*x+z*z);
    h.Alpha = acos((L1*L1+AC*AC-L2*L2)/(2*L1*AC))*180/PI+acos((x*x+AC*AC-z*z)/(2*x*AC))*180/PI;
    h.Beta = acos((L1*L1+L2*L2-AC*AC)/(2*L1*L2))*180/PI;
  }else
  {
   
    h.Alpha = 90+acos((L1*L1+z*z-L2*L2)/(2*L1*z))*180/PI;
    h.Beta = acos((L1*L1+L2*L2-z*z)/(2*L1*L2))*180/PI;
  }
}

void ledc_Initialization()  //ledc初始化程序
{
   
  ledcSetup(channel0, freq0, resolution0); // 设置通道0
  ledcSetup(channel1, freq1, resolution1); // 设置通道1
  ledcSetup(channel2, freq2, resolution2); // 设置通道2
  ledcSetup(channel3, freq3, resolution3); // 设置通道3
  ledcSetup(channel4, freq4, resolution4); // 设置通道4
  ledcSetup(channel5, freq5, resolution5); // 设置通道5
  ledcSetup(channel6, freq6, resolution6); // 设置通道6
  ledcSetup(channel7, freq7, resolution7); // 设置通道7
  ledcSetup(channel8, freq8, resolution8); // 设置通道8
  ledcSetup(channel9, freq9, resolution9); // 设置通道9
  ledcSetup(channel10, freq10, resolution10); // 设置通道10
  ledcSetup(channel11, freq11, resolution11); // 设置通道11
  
  ledcAttachPin(DJ0_PWM, channel0);  // 将通道0与引脚13连接
  ledcAttachPin(DJ1_PWM, channel1);  // 将通道1与引脚12连接
  ledcAttachPin(DJ2_PWM, channel2);  // 将通道2与引脚14连接
  ledcAttachPin(DJ3_PWM, channel3);  // 将通道3与引脚27连接
  ledcAttachPin(DJ4_PWM, channel4);  // 将通道4与引脚26连接
  ledcAttachPin(DJ5_PWM, channel5);  // 将通道5与引脚25连接
  ledcAttachPin(DJ6_PWM, channel6);  // 将通道6与引脚33连接
  ledcAttachPin(DJ7_PWM, channel7);  // 将通道7与引脚32连接
  ledcAttachPin(DJ8_PWM, channel8);  // 将通道8与引脚21连接
  ledcAttachPin(DJ9_PWM, channel9);  // 将通道9与引脚19连接
  ledcAttachPin(DJ10_PWM, channel10);  // 将通道10与引脚23连接
  ledcAttachPin(DJ11_PWM, channel11);  // 将通道11与引脚22连接
}


void ledc_dutycycle0()  //比较角度设置占空比
{
     
  duty0 = 69.7;   // 设置初始占空比
  duty1 = 69.7;  // 设置初始占空比
  duty2 = 69.7;   // 设置初始占空比
  Initial_Alpha0 = 135;  //设置初始角度
  Initial_Beta0 = 90;  //设置初始角度

  if(h.Alpha>Initial_Alpha0)
  {
   
    duty1=duty1+(h.Alpha-Initial_Alpha0)*0.71;  
  }
  if(h.Alpha<Initial_Alpha0)
  {
   
    duty1=duty1-(Initial_Alpha0-h.Alpha)*0.71;  
  }
  if(h.Beta>Initial_Beta0)
  {
   
    duty2=duty2-(h.Beta-Initial_Beta0)