平衡小车速度环使用 PI（比例积分）控制器，这也是速度控制最常使用的控制器。PI 控制器是一种线性控制器，它根据给定值与实际输出值构成控制偏差，将偏差的比例（P）和积分（I）通过线性组合构成控制量，对被控对象进行控制。
下面是速度 PI 控制的代码（不包括遥控部分， 遥控部分后面再单独讲解）：
2. /***************************************************************
3. ** 函数名称: SpeedControl
4. ** 功能描述: 速度环控制函数
5. ** 输 入:
6. ** 输 出:
7. ** 全局变量:
8. ** 作 者: 喵呜实验室MiaowLabs
9. ** 淘 宝： https://miaowlabs.taobao.com/
10. ** 日 期: 2014年08月01日
11. ***************************************************************/
12.
13. void SpeedControl(void)
14. {
15. float fP,fI;
16. float fDelta;
17.
18.
19. g_fCarSpeed = (g_s32LeftMotorPulseSigma + g_s32RightMotorPulseSigma ) * 0.5 ;
20. g_s32LeftMotorPulseSigma = g_s32RightMotorPulseSigma = 0; //全局变量 注意及时清零
21.
22. g_fCarSpeed = 0.7 * g_fCarSpeedOld + 0.3 * g_fCarSpeed ;//低通滤波，使速度更平滑
23. g_fCarSpeedOld = g_fCarSpeed;
24.
25. fDelta = CAR_SPEED_SET;
26. fDelta -= g_fCarSpeed;
27.
28. fP = fDelta * g_tCarSpeedPID.P;
29. fI = fDelta * g_tCarSpeedPID.I;
30.
31. g_fCarPosition += fI;
32. g_fCarPosition += g_fBluetoothSpeed;
33.
34. //积分上限设限
35. if((s16)g_fCarPosition > CAR_POSITION_MAX) g_fCarPosition = CAR_POSITION_MAX;
36. if((s16)g_fCarPosition < CAR_POSITION_MIN) g_fCarPosition = CAR_POSITION_MIN;
37.
38. g_fSpeedControlOutOld = g_fSpeedControlOutNew;
39. g_fSpeedControlOutNew = fP + g_fCarPosition;
40. }
41. /***************************************************************
42. ** 函数名称: SpeedControlOutput
43. ** 功能描述: 速度环控制输出函数-分多步逐次逼近最终输出，尽可能将对直立环的干扰降低。
44. ** 输 入:
45. ** 输 出:
46. ** 全局变量:
47. ** 作 者: 喵呜实验室MiaowLabs
48. ** 淘 宝： https://miaowlabs.taobao.com/
49. ** 日 期: 2014年08月01日
50. ***************************************************************/
51. void SpeedControlOutput(void)
52. {
53. float fValue;
54. fValue = g_fSpeedControlOutNew - g_fSpeedControlOutOld ;
55. g_fSpeedControlOut = fValue * (g_u8SpeedControlPeriod + 1) / SPEED_CONTROL_PERIOD + g_fSpeedControlOutOld;
56. }
以上代码实现的效果是：使小车在保持平衡的同时速度为零。
调试过程包括计算速度偏差、确定 kp 和 ki 值的极性（也就是正负号） 与大小。
小车速度测量值g_fCarSpeed我们使用左右编码器之和再除以2表示，我们对速度值进行低通滤波，具体的系数由工程经验得到。这样做的目的是为了减缓速度值的变化，防止速度控制对直立造成干扰，根据公式“偏差=目标值-测量值”，我们的目标速度CAR_SPEED_SET设置为零。然后，将速度控制分成两个函数，一个主要负责计算，一个主要输出，输出时分多步逐次逼近最终输出，比如将5ms输出一次的PWM计算值分成5个1ms缓和输出，尽可能将对直立环的干扰降低，因为平衡小车系统里面，直立控制是主要的，其他控制对于直立来说都是一种干扰。
为了调试方便，接下来我们先关闭之前已经调试好的直立控制部分， 如下图所示：
1. void SysTick_Handler(void)
2. {
3. SoftTimerCountDown(); //软定时器
4.
5. g_u8MainEventCount++;
6.
7. g_u8SpeedControlPeriod++;
8. SpeedControlOutput(); //速度环控制输出函数，每1ms执行一次
9. if(g_u8MainEventCount>=5)
10. {
11. g_u8MainEventCount=0;
12. GetMotorPulse(); //捕获电机脉冲（速度）函数，每5ms执行一次
13. }
14. else if(g_u8MainEventCount==1)
15. {
16. //MPU6050_Pose(); //读取MPU6050数据函数，每5ms执行一次
17. //AngleCalculate(); //角度环计算函数，每5ms执行一次
18. }
19. else if(g_u8MainEventCount==2)
20. {
21. //AngleControl(); //角度环控制函数，每5ms执行一次
22.
23. }
24. else if(g_u8MainEventCount==3)
25. {
26. g_u8SpeedControlCount++;
27. if(g_u8SpeedControlCount >= 5)//25ms
28. {
29. SpeedControl(); //车模速度控制函数，每25ms调用一次
30. g_u8SpeedControlCount=0;
31. g_u8SpeedControlPeriod=0;
32. }
33. }
34. else if(g_u8MainEventCount==4)
35. {
36. //MotorManage(); //电机使能/失能控制函数，每5ms执行一次
37. MotorOutput(); //电机输出函数，每5ms执行一次
38. }
39. }
积分项由偏差的积分得到，所以积分控制和比例控制的极性是相同的，而根据工程经验，在不同的系统中，PID参数相互之间会有一定的比例关系。在我们的平衡小车速度控制系统里面，一般我们可以把 ki 值设置为 ki=kp/150，只要我们可以得到kp值的大小和极性，就可以完成速度控制部分的参数整定了。显然，这样大大缩短了 PID 参数整定的时间。
我们通过 STM32 定时器的正交编码器接口模式对编码器捕捉脉冲，使用M法测速（每5m内捕捉的脉冲数）得到小车的速度信息，通过观察数据，我们发现两路编码器相加最大值在150左右，而考虑到系统的反应时间，我们假定当速度偏差达到最大速度100%的时候，系统输出电机的最快速度，再根据1000代表占空比 100%，我们可以大概估算kp 最大值=1000/(150*100%)=6.6。
另外要说明的是，虽然这里的PI控制器是速度控制常用的一种控制器，但是和普通的调速系统不一样，这里的速度控制是正反馈的， 当小车以一定的速度运行的时候，我们要让小车停下来，小车需要行驶更快的速度去“追”，小车运行的速度越快，去“追”的速度也就越快，所以这是一个正反馈的效果。如果使用常规的速度负反馈，当小车以一定的速度运行的时候，我们通过减速让小车慢下来，小车会因为惯性向前倒下。
下面介绍一种确定速度控制是正反馈还是负反馈的方法。根据之前的估计，先设定 kp=-6.6， ki=kp/150。当我们拿起小车，旋转其中一个小车轮胎的时候，根据我们设定的速度偏差fDelta = CAR_SPEED_SET-g_fCarSpeed；另外一个车轮会反向转动，让偏差趋向于零。这就是常规的速度控制里面的负反馈，不是我们需要的效果。接下来设定 kp=6.6，ki=kp/150。此时，当我们旋转其中一个小车轮胎的时候，两个轮胎会往相同的方向加速，直至电机的最大速度，这是典型的正反馈效果，也是我们期望看到的。至此，我们可以确定 kp、ki 的符号应该是正的。
下面我们进行平衡小车速度控制 kp 与 ki 值的整定，此时需要打开直立环，因为我们需要结合直立环观察速度环对直立环的影响，如下图所示
在平衡小车速度控制系统里面，一般我们可以把 ki 值设置为 ki=kp/150。所以，我们只需要对 kp 值进行整定即可。在调试的过程中设定速度控制的目标为零，所以，调试的理想结果应该是：小车保持平衡的同时，速度接近于零。实际上，因为小车存在比较大的转动惯量和惯性，并且齿轮减速器存在死区，很难调试到让小车完全保持静止的，我们调试平衡小车只是为了学习 PID 控制算法，所以，没有必要花太多的时间去调参数，让小车完全静止，只要能够大概实现我们需要的功能，并在这个过程对 PID有进一步的了解即可。
首先，设定 kp=1，ki=kp/150，这个时候我们可以看到，小车的速度控制比较,弱，很难让速度恒定。
设定 kp=3，ki=kp/150，这个时候我们可以看到，小车的速度控制的响应有所加快，但是来回摆动还是有点大，还是不足以让小车保持接近于静止的状态。
设定 kp=6,ki=kp/150 ，这个时候我们可以看到，小车已经性能很完美了，我们接下来尝试加大 kp 值看一下效果。
设定 kp=9,ki=kp/150 ，这个时候我们可以看到，小车虽然回正力度增大了，而且响应更加快了，但是稍微加入一点的干扰都会让小车大幅度摆动，抗干扰能力明显不足，所以这组参数不可取。 至此，我们可以确定得到 kp=6，kd=0.04是速度控制 P、 I 参数的理想值。
我们再来体验一下速度控制负反馈在平衡小车里面的效果，设定kp=-6，ki=kp/150，这个时候我们可以看到， 小车会迅速往一个方向倒下。也就是说常规的速度负反馈在我们这边是“帮倒忙” 了！
至此，速度控制调试部分就告一段落了，如果要加入遥控前进后退功能的话，遥控的速度通过积分融入速度控制器，减缓速度改变对直立控制的影响。
这样只需要通过蓝牙在 usart3.c 中的串口3接收中断函数改变g_fBluetoothSpeed的值就可以改变小车的速度，实现前进后退了。
积分限幅是增加了遥控之后必不可少的，如果没有积分限幅，就无法限制小车的最大前进速度。这样在遥控的过程中，小车很容易倒下。换句话说积分的最大赋值决定了小车的最大前进速度，而g_fBluetoothSpeed值决定了小车的给定速度。