一起玩儿平衡车（ESP32）——07 平衡车主程序的编写

摘要：本文介绍如何编写平衡车的主程序

在开发小车的主程序之前，先来了解一下平衡小车在什么情况下是直立不倒的？怎样做才能保持平衡不倒？

要严谨的解释这两个问题，可能需要经过周密的受力分析和理论推导，但那样太抽象了，即使能说得明白，恐怕能真正理解的也不多。下面就用我自己的语言来简单的解释一下这两个问题。

小时候，有一个杂技项目叫“耍中幡”不知道大家见过没有？就是一个人会托着一个长长的装饰得非常漂亮的杆子，上面还有铃铛，通过人体的不断移动，保持杆子（学名叫幡）不倒。那时候的小伙伴也经常尝试手托着一根棍子尽量保持竖直不倒，以至于老师的教鞭、扫地的笤帚等都成了我们练习的道具。保持倒立物不倒的根本就在要尽量保持物体的重心在支撑点的正上方。这样在物理上分析就是竖直向下的重力与向上的支持力保持平衡，物体会处于静止状态。一旦物体的中心发生了偏离，我们就要迅速的调整支撑点的位置，让重心重新回到支撑点的正上方，那么倒立物就不会倒了。

回到我们的平衡车，也就是当平衡车的中心，在车轴的正上方时，车子会保持稳定，不会向左右两侧倒。当车向左边倒的时候，重心向左偏移，这个时候的轮子必须要加速逆时针旋转，让支撑点——车轴，以更快的速度向左移动，这样重心就会重新回到车轴的正上方，小车就不会继续往左倒了，但如果车轴的移动速度不够，始终无法让重心回到正上方，最终小车就无法再回到平衡位置了。

所以，最关键的就是要记住结论，当车往左侧倒时，车轮要逆时针转动，推动小车向左运动。当车往右侧倒时，车轮要顺时针转动，推动小车向右运动。

前面已经介绍了整个程序的流程，不是很复杂，空白行以及注释都算上，整个代码也不到100行，如下所示：

#include "BluetoothSerial.h" #include <MPU6050_tockn.h> #include <Wire.h> #include "car.h" /** ESP32引脚定义**/ #define MPU6050_SDA 18 #define MPU6050_SCL 19 int pwm = 0; Car car; float BalanceAngle = 0;                                  //静态机械平衡角度 float kp = 1, ki = 0, kd = 0;                                 // PID参数默认值 float KeepAngle, bias, integrate;                  // 保持角度，角度偏差，偏差积分量 float AngleX, GyroX;  // mpu6050输出的数据 int verticalPWM;     //各种PWM计算结果 BluetoothSerial SerialBT; MPU6050 mpu6050(Wire); // 直立环PID计算 void verticalPwmCalculation() {   AngleX = mpu6050.getAngleX();   GyroX = mpu6050.getGyroX();   bias = AngleX - KeepAngle;  // 计算角度偏差。bias为小车角度与静态平衡角度的差值。   integrate += bias;           // 偏差的积分，integrate为全局变量，一直累积。   integrate = constrain(integrate, -1000, 1000);   /** 直立PID的计算*/   verticalPWM = kp * bias + ki * integrate + kd * GyroX; } // 控制电机方法 void motorControl(int leftPWM, int rightPWM) {   if (AngleX > 50 || AngleX < -50) {  // 倾角过大，停止马达输出     car.motor(0, 0);     return;   }   // 控制马达输出   car.motor(leftPWM, rightPWM); } void setup() {   SerialBT.begin("Balance CAR");   Serial.begin(115200);   Wire.begin(MPU6050_SDA, MPU6050_SCL);   mpu6050.begin();   mpu6050.calcGyroOffsets(true);   // 车初始化   car.begin();   KeepAngle = BalanceAngle;  //平衡角度初始化   car.motor(0, 0);   Serial.println("Init OK......"); } void loop() {   mpu6050.update();  //陀螺仪刷新   verticalPwmCalculation();  //计算直立环PWM   motorControl( verticalPWM, verticalPWM);  //马达输出   delay(10); }

下面来给大家简单的介绍一下这个程序的代码。初始化部分，是setup()函数，按照之前介绍的那样，完成了串口（包括蓝牙串口）初始化，小车类对象的初始化以及MPU6050模块的初始化工作。

在主函数中，依次完成了更新姿态传感器数据、用PID算法计算控制PWM的量，然后按PID计算结果驱动电机。

在这里需要提醒的是，需要注意一下MPU6050的安装方向，我的安装结果是MPU的X轴是垂直与车轴的，所以在判断车辆是否倾倒的时候使用X轴角度和X轴加速度就可以了。如果有人是旋转90度安装的，也就是X轴与车轴平行，Y轴与车轴垂直，那么就需要使用Y轴角度和Y轴加速度来进行PID计算。不要斜着安装MPU6050模块，那样需要X、Y轴矢量求和才能得到小车倾斜的角度和角加速度，无疑增加了工作量。

上边这个程序如果有调试好的PID参数，就是可以运行的程序了。但现在调试还没有进行，所以还需要一个调整参数的函数，使得我们可以使用串口来不断的调整Kp、Ki和Kd参数的值，直到找到合适的参数组合。

调整参数的方法与上一篇中测试电机死区的方法类似，如下所示：

void serialDebug() {   if (SerialBT.available() > 0||Serial.available()>0) {     //delay(5);     char CMD = SerialBT.available()>0?SerialBT.read():Serial.read();     Serial.println(CMD);     switch (CMD) {       // 直立环调节       case '7':  // 调节直立环比例项         kp += 1;         break;       case '4':         kp -= 1;         break;       case '8':  // 调节直立环积分项         ki += 0.1;         break;       case '5':         ki -= 0.1;         break;       case '9':  // 调节直立环微分项         kd += 0.1;         break;       case '6':         kd -= 0.1;         break;       case 'w':  // 测试车轮         car.motor(200, 200);         delay(5000);         break;       case 'm':  // 测试mpu6050         SerialBT.print("AngleX:");         SerialBT.println(AngleX);         break;     }     if (kp < 0) kp = 0;     if (ki < 0) ki = 0;     if (kd < 0) kd = 0;     SerialBT.print("Keep Angle:");     SerialBT.println(KeepAngle);     SerialBT.print("kp:");     SerialBT.print(kp);     SerialBT.print("ki:");     SerialBT.print(ki);     SerialBT.print("kd:");     SerialBT.print(kd);     SerialBT.println("===============");   } }

在这个修改调试参数的方法中，是通过发送字符来改变参数值的，其中“7”和“4”用来增减kp参数的值，“8”和“5”用来增减参数ki的值。“9”和“6”用来增减kd参数的值。其实这很好记忆，所用的按键就是小键盘上前两排数字按键，每列调整一个参数，上边的按键增加，下边的按键减少，从左到右依次调整kp、ki和kd的值。

最后在loop()函数中，加入对修改参数的方法的调用就可以了。到此为止，拥有直立功能的平衡车的全部代码就编写完成了，纯代码数量不超过200行，应该是很好理解了。

下一篇将带领大家进行PID参数调试，再见！