基于esp32的平衡车代码（三环控制）

#include <Arduino.h>
#include "BluetoothSerial.h"
#include <MPU6050_tockn.h>
#include <Wire.h>
#include<Ticker.h>
Ticker timer_read_encoder; 
BluetoothSerial SerialBT;                  //实例化esp32的蓝牙串口对象
MPU6050 mpu6050(Wire);                     //实例化mpu6050对象
  /*【编码器宏定义】*/
#define ENCODER_L       16    
#define DIRECTION_L     17
#define ENCODER_R       18
#define DIRECTION_R     23
#define interrupt_time  20   //中断时间
  /*【引脚宏定义】*/
#define IN1       26         //0号通道
#define IN2       27         //1号通道
#define IN3       32         //2号通道
#define IN4       33         //3号通道
#define sda_pin   13         //SDA
#define scl_pin   15         //SCL
#define leftOffset     50
#define rightOffset    40
  /*【直立环变量】*/
float Balance_Angle_raw =-1.8;         //机械平衡角度   
float kp=5.0;
float kd=0.74;  
float bias; 
float AngleX;                          //X轴角度  
float GyroX;                           //X轴角速度    
  /*【转向环变量】*/
float turn_kp=-0.4;                    
float turn_spd;                        //转向速度目标值 
float GyroZ;                           //Z轴的角速度
  /*【速度环变量】*/
float V_kp=1.2;
float V_ki=0.009; 
float Go_V=0;                                    //小车速度
float error;                                     //速度偏差
float integrate;                                 //偏差的积分
int   error_last;                                //小车上一次速度的偏差
float error_filter;                              //过滤过的速度偏差
int32_t Velocity_L, Velocity_R;                  //左右轮编码器数据
int32_t Velocity_Left, Velocity_Right;           //左右轮速度
  /*【PWM变量】*/
int L_PWM;
int R_PWM;
int vertical_PWM;                  //直立PWM   
int v_PWM;                         //速度PWM
int turn_PWM;                      //转向PWM
int PWM;                           //总PWM 
void READ_ENCODER_L(void)                  {//读取左边编码器的数据
  if (digitalRead(ENCODER_L) == LOW) {                       //如果是下降沿触发的中断
    if (digitalRead(DIRECTION_L) == LOW)      Velocity_L--;  //根据另外一相电平判定方向
    else      Velocity_L++;
    }
  else {     //如果是上升沿触发的中断
    if (digitalRead(DIRECTION_L) == LOW)      Velocity_L++; //根据另外一相电平判定方向
    else     Velocity_L--; 
  }
}
void READ_ENCODER_R(void)                  {//读取右边编码器的数值
  if (digitalRead(ENCODER_R) == LOW) { //如果是下降沿触发的中断
    if (digitalRead(DIRECTION_R) == LOW)      Velocity_R--;//根据另外一相电平判定方向
    else      Velocity_R++;
  }
  else {   //如果是上升沿触发的中断
    if (digitalRead(DIRECTION_R) == LOW)      Velocity_R++; //根据另外一相电平判定方向
    else     Velocity_R--;
  }
}
void read(void)                            {//计算小车的速度
    Velocity_Left = Velocity_L;    Velocity_L = 0;  //读取左轮编码器数据，并清零，这就是通过M法测速（单位时间内的脉冲数）得到速度。
    Velocity_Right = Velocity_R;    Velocity_R = 0; //读取右轮编码器数据，并清零
}
void motor(int left_EN, int right_EN)      {//马达驱动函数

  left_EN = constrain(left_EN, -255, 255);
  right_EN = constrain(right_EN, -255, 255);  //限定PWM区间在-255~255
  if (left_EN >= 0)
  {
    ledcWrite(0, left_EN);
    ledcWrite(1, 0);
  }
  if (left_EN < 0)
  {
    ledcWrite(0, 0);
    ledcWrite(1, 0 - left_EN);
  }
  if (right_EN >= 0)
  {
    ledcWrite(2, right_EN);
    ledcWrite(3, 0);
  }
  if (right_EN < 0)
  {
    ledcWrite(2, 0);
    ledcWrite(3, 0 - right_EN);
  }
}
void serial_debug()                        {//蓝牙串口调试和控制函数，根据手机端发送来的串口数据调试或控制
  if (SerialBT.available()> 0){
    char DATA = SerialBT.read();
    delay(5);
    switch (DATA)
    {
      /*【调节平衡点】*/
      case 'u':Balance_Angle_raw += 0.1;  break;
      case 'd':Balance_Angle_raw -= 0.1;  break; 
      /*【调节直立环】*/
      case '0':kp -= 0.2;                 break;
      case '1':kp += 0.2;                 break;
      case '2':kd -= 0.01;                break;
      case '3':kd += 0.01;                break;
      /*【调节速度环】*/
      case '4':V_kp-= 0.02;                break;
      case '5':V_kp += 0.02;               break;   
      case '6':V_ki -= 0.0001;             break;
      case '7':V_ki += 0.0001;             break;  
      /*【调节转向环】*/
      case '8':turn_kp -= 0.1;            break;
      case '9':turn_kp += 0.1;            break;
      /*【小车指令控制】*/
      case 's':Go_V=0;integrate=0;            break;     //停止
      case 'a':Go_V=10;integrate=0;           break;     //前进
      case 'b':Go_V=-10;integrate=0;          break;     //后退
      case 'z':turn_spd = 0;integrate=0;      break;     //不转向
      case 'l':turn_spd = 300;                break;     //左转
      case 'r':turn_spd = -300;               break;     //右转
    }
    /*【蓝牙打印输出】*/
    SerialBT.print("Balance_Angle_raw: ");SerialBT.println(Balance_Angle_raw);
    SerialBT.println("直立环控制参数：");
    SerialBT.print("kp:");SerialBT.print(kp);SerialBT.print(" kd:");SerialBT.println(kd);
    SerialBT.println("速度环控制参数：");
    SerialBT.print("V_kp:");SerialBT.print(100*V_kp);SerialBT.print("V_ki:");SerialBT.println(100*V_ki);
    SerialBT.println("转向环控制参数：");
    SerialBT.print("  turn_kp:");SerialBT.println(turn_kp);   
    SerialBT.print(" PWM:");SerialBT.print(rightOffset);SerialBT.println("------"); 
  }
}
void vertical_pwm_calculation()            {//直立PMW计算，采用PD控制
  AngleX = mpu6050.getAngleX();            //获取X轴的角度
  GyroX = mpu6050.getGyroX();              //获取X轴的角速度              
  bias = AngleX - Balance_Angle_raw;       // 计算角度偏差
  vertical_PWM = kp*bias + kd * GyroX;     //计算实时的PWM脉冲。
}
void turn_pwm_calculation()                {//转向PMW计算
  GyroZ = mpu6050.getGyroZ();              //获取陀螺仪Z轴角速度
  turn_PWM = turn_kp * (turn_spd - GyroZ);
}
void v_pwm_calculation()                   {//速度PWM计算，采用PI控制
  error=(Velocity_Left+Velocity_Right)/2-Go_V;   //计算偏差
  error_filter=0.3*error+0.7*error_last;         //一阶滤波
  error_last=error;                              //将测得的偏差变成上一次的偏差，供下次利用
  integrate=integrate+error_filter;              //将过滤过的偏差积分
  integrate = constrain(integrate,-1500,1500);   //限定误差积分的最大和最小值
  v_PWM=V_kp*error+V_ki*integrate;               //计算速度的PWM
}
void motor_control()                       {//PWM补偿，转向PWM，并控制马达输出，让电机转动
  /*【死区补偿】*/
  if (PWM > 0){L_PWM = PWM + leftOffset;R_PWM = PWM + rightOffset;}
  if (PWM < 0){L_PWM = PWM - leftOffset;R_PWM = PWM - rightOffset;}          
  if (PWM == 0){L_PWM = 0; R_PWM = 0;} 
  /*【转向控制】*/
  L_PWM -= turn_PWM;
  R_PWM += turn_PWM; 
  /*【输出限幅】*/
  L_PWM = constrain(L_PWM, -255, 255); 
  R_PWM = constrain(R_PWM, -255, 255);
  /*【判断状态】*/
  if (AngleX > 45 || AngleX < -45){motor(0, 0);}
  /*【马达输出】*/
  motor(L_PWM, R_PWM);
}
void channel()                             {//PWM通道函数
  ledcSetup(0,1000,8);ledcAttachPin(IN1,0);//设置通道0
  ledcSetup(1,1000,8);ledcAttachPin(IN2,1);//设置通道1
  ledcSetup(2,1000,8);ledcAttachPin(IN3,2);//设置通道2
  ledcSetup(3,1000,8);ledcAttachPin(IN4,3);//设置通道3
}
void Encoder_Init()                        {//编码器初始化
  pinMode(ENCODER_L, INPUT);       //编码器引脚 输入模式
  pinMode(ENCODER_R, INPUT);       //编码器引脚 输入模式
  pinMode(DIRECTION_L, INPUT);     //编码器引脚 输入模式
  pinMode(DIRECTION_R, INPUT);     //编码器引脚 输入模式 
                                                         //编码器接口1 开启外部跳边沿中断 
  attachInterrupt(ENCODER_L, READ_ENCODER_L, CHANGE);    //中断函数READ_ENCODER_L
                                                         //编码器接口2 开启外部跳边沿中断 
  attachInterrupt(ENCODER_R, READ_ENCODER_R, CHANGE);    //中断函数READ_ENCODER_R
  interrupts();                                          //打开外部中断      
  timer_read_encoder.attach_ms(interrupt_time, read);    //设置20毫秒读取一次read函数
}                                  
void setup()                               {//初始化函数
  channel();                           //选择通道
  Serial.begin(9600);                  //串口初始化
  SerialBT.begin("ESP32_car");         //蓝牙初始化
  Wire.begin(sda_pin, scl_pin);        //I2C初始化
  Encoder_Init();                      //编码器初始化
  mpu6050.begin();                             //mpu6050初始化
  mpu6050.calcGyroOffsets(true);               //mpu6050数值补偿
  motor(0, 0);                                 //马达驱动初始化
  delay(10);                                   //延时
}
void loop()                                {//主函数
  serial_debug();                //串口调试+控制
  mpu6050.update();              //陀螺仪刷新
  v_pwm_calculation();           //速度环PWM计算
  vertical_pwm_calculation();    //直立环PWM计算
  turn_pwm_calculation();        //转向PWM计算
  PWM = vertical_PWM+kp*v_PWM;   //直立环和速度环的串级PID控制，将速度环的输出量输入到角度环，从而实现对速度的控制
  motor_control();               //最终由将得到的PWM给到马达，控制马达的输出
  Serial.print("integrate");Serial.print(integrate);
  Serial.print("PWM");Serial.print(PWM);
  Serial.print("v_PWM");Serial.println(kp*v_PWM);
}

 PID部分参数需自行修改