运动速度闭环外环PID控制
在只有角度环（又称直立环）的情况下，小车就如同漂浮在海面上的冰山，在理想情况下，是可以实现直立的，但是没有抗干扰能力，很容易在受到外力干扰的情况发生偏移。
在 control.c 文件中里新增以下定义：
#define CAR_SPEED_SET 0//小车目标速度
#define CAR_POSITION_MAX 900//路程上限
#define CAR_POSITION_MIN (-900)
#define SPEED_CONTROL_PERIOD 25 //速度环控制周期25ms
float g_fCarSpeed;//小车实际速度
float g_fCarSpeedPrev;//小车前一次速度
float g_fCarPosition;//小车路程
long g_lLeftMotorPulseSigma;//左电机25ms内累计脉冲总和
long g_lRightMotorPulseSigma;//右电机25ms内累计脉冲总和
float g_fSpeedControlOut;//速度环输出
float g_fSpeedControlOutOld;//上次速度环计算量
float g_fSpeedControlOutNew;//本次速度环计算量
unsigned int g_nSpeedControlCount;//速度环控制计数
float g_fSpeedControlOut,g_fSpeedControlOutNew,g_fSpeedControlOutOld;//速度环输出
int g_nSpeedControlPeriod;//速度环控制周期计算量
float g_fSpeed_P = 0.6;//速度环P参数
float g_fSpeed_I = 0.03;//速度环I参数
新增速度环控制函数代码：
void SpeedControl(void)//速度外环控制函数
{
float fP,fI;
float fDelta;//临时变量，用于存储误差
g_fCarSpeed = (g_lLeftMotorPulseSigma + g_lRightMotorPulseSigma ) * 0.5;//左轮和右轮的速度平均值等于小车速度
g_lLeftMotorPulseSigma = g_lRightMotorPulseSigma = 0;//全局变量，注意及时清零
g_fCarSpeed = 0.7 * g_fCarSpeedPrev + 0.3 * g_fCarSpeed ;//低通滤波，使速度更平滑
g_fCarSpeedPrev = g_fCarSpeed; //保存前一次速度
fDelta = CAR_SPEED_SET;
fDelta -= g_fCarSpeed;//误差=目标速度-实际速度
fP = fDelta * g_fSpeed_P;
fI = fDelta * g_fSpeed_I;
g_fCarPosition += fI;
//设置积分上限设限
if((int)g_fCarPosition > CAR_POSITION_MAX) g_fCarPosition = CAR_POSITION_MAX;
if((int)g_fCarPosition < CAR_POSITION_MIN) g_fCarPosition = CAR_POSITION_MIN;
g_fSpeedControlOutOld = g_fSpeedControlOutNew;//保存上一次输出
g_fSpeedControlOutNew = fP + g_fCarPosition; //PI控制，P计算值和I计算值相加
}
void SpeedControlOutput(void)//速度外环平滑输出函数
{
float fValue;
fValue = g_fSpeedControlOutNew - g_fSpeedControlOutOld ;//速度计算量差值=本次速度计算量-上次速度计算量
g_fSpeedControlOut = fValue * (g_nSpeedControlPeriod + 1) / SPEED_CONTROL_PERIOD + g_fSpeedControlOutOld;
}
g_fSpeedControlOutNew 为最新一次速度环控制的 PID 输出， g_fSpeedControlOutOld 为上一次速度环 PID 控制输出值，fValue 为两者之间差值，求出此次 PID 输出值较上一次变化了多少，然后按比例将变化的 fValue 值逐渐加到上一次 PID 输出值，从而得到最新的当前 PID 输出值。这里面的参数设置尤为重要，我要分多少份加，多长时间加完等等。首先来看多长时间内加完，这取决于控制周期，一定要小于等于控制周期才可以。比如说我当前的速度环控制周期为 25ms，那么我就要必须在 25ms 内将之前的 PID 输出值加到最新的 PID 输出值，所以我这里将 fvalue 值分成 25 份，每 1ms 执行一次平滑函数，每执行一次平滑函数都在之前的基础上增加 4%，这样 fvalue 值全部加完刚好是 25ms，当然要想形成每 ms 增加 4%，g_nSpeedControlPeriod 这个参数也要在 1ms 中断中不断++，直到 25ms 时间到执行速度环控制函数时将其清零。平滑地逐步逼近输出最后的计算值。
这段代码的意思：速度外环平滑输出函数，速度的 PWM 改变量如果在 25ms 时刻计算出后立刻输出，会造成不平滑抖动等，这段代码本意就是把这个 25ms 周期计算一次得到的 PWM 分配到 25 个 1ms 时间去输出，平滑地逐步逼近输出最后的计算值。但是又因为 MotorOutput() 是 5ms 才运行一次，平滑输出函数的实际效果变成了将 25ms周期计算到的 PWM 分配到 5 个 5ms 时间去输出，平滑地逐步逼近输出最后的计算值！
修改电机输出函数，加入速度环控制量：
void MotorOutput(void)//电机输出函数,将直立控制、速度控制、方向控制的输出量进行叠加,并加入死区常量，对输出饱和作出处理。
{
g_fLeftMotorOut = g_fAngleControlOut - g_fSpeedControlOut;//这里的电机输出等于角度环控制量 + 速度环外环,这里的 - g_fSpeedControlOut 是因为速度环的极性跟角度环不一样，角度环是负反馈，速度环是正反馈
g_fRightMotorOut = g_fAngleControlOut - g_fSpeedControlOut;
/*增加电机死区常数*/
if((int)g_fLeftMotorOut>0) g_fLeftMotorOut += MOTOR_OUT_DEAD_VAL;
else if((int)g_fLeftMotorOut<0) g_fLeftMotorOut -= MOTOR_OUT_DEAD_VAL;
if((int)g_fRightMotorOut>0) g_fRightMotorOut += MOTOR_OUT_DEAD_VAL;
else if((int)g_fRightMotorOut<0) g_fRightMotorOut -= MOTOR_OUT_DEAD_VAL;
/*输出饱和处理，防止超出PWM范围*/
if((int)g_fLeftMotorOut > MOTOR_OUT_MAX) g_fLeftMotorOut = MOTOR_OUT_MAX;
if((int)g_fLeftMotorOut < MOTOR_OUT_MIN) g_fLeftMotorOut = MOTOR_OUT_MIN;
if((int)g_fRightMotorOut > MOTOR_OUT_MAX) g_fRightMotorOut = MOTOR_OUT_MAX;
if((int)g_fRightMotorOut < MOTOR_OUT_MIN) g_fRightMotorOut = MOTOR_OUT_MIN;
SetMotorVoltageAndDirection((int)g_fLeftMotorOut,(int)g_fRightMotorOut);
}
修改 GetMotorPulse() 读取电机脉冲函数，修正两个电机脉冲数据的极性，添加速度外环相关代码：
void GetMotorPulse(void)//读取电机脉冲
{
g_nRightMotorPulse = (short)(__HAL_TIM_GET_COUNTER(&htim4));//获取计数器值
g_nRightMotorPulse = (-g_nRightMotorPulse);
__HAL_TIM_SET_COUNTER(&htim4,0);//TIM4计数器清零
g_nLeftMotorPulse = (short)(__HAL_TIM_GET_COUNTER(&htim2));//获取计数器值
__HAL_TIM_SET_COUNTER(&htim2,0);//TIM2计数器清零
g_lLeftMotorPulseSigma += g_nLeftMotorPulse;//速度外环使用的脉冲累积
g_lRightMotorPulseSigma += g_nRightMotorPulse;//速度外环使用的脉冲累积
}
注意上面的代码，增加了一句 g_nRightMotorPulse = (-g_nRightMotorPulse);，这是因为两个电机装在两轮自平衡小车是相对着安装的，当小车前进时，一个电机的转动方向是另一个电机的反方向，这时读取到的编码器数据，一个是正数，另一个是负数，而负号代表反转。所以，在这里我们要在软件上修正两个编码器数据，我们定义两轮自平衡小车往一个方向运动为正方向，编码器为正数，我们在此将那个编码器为负数的数据取反，拨反为正。
g_lLeftMotorPulseSigma += g_nLeftMotorPulse;//速度外环使用的脉冲累积
g_lRightMotorPulseSigma += g_nRightMotorPulse;//速度外环使用的脉冲累积
而上面这两句代码，是因为速度外环的控制频率是比角度环低的，等下看到中断代码，会看到速度环的控制频率是 40Hz（25ms），而角度环的控制频率是 200Hz（5ms）。而 GetMotorPulse() 读取电机脉冲函数是每 5ms 读取一次脉冲，这里两句代码将 5 次 5ms 的脉冲累计起来。
在 control.h 头文件中加入代码：
extern unsigned int g_nSpeedControlCount;
extern int g_nSpeedControlPeriod;//速度环控制周期计算量
声明外部用到的变量。同时加入以下声明：
void SpeedControl(void);
void SpeedControlOutput(void);
声明变量，等下中断里要调用。
在 stm32fxx_it.c 中更新以下代码：
void SysTick_Handler(void)
{
/* USER CODE BEGIN SysTick_IRQn 0 */
g_nMainEventCount++;//每进一次中断，主事件函数自动加1
g_nSpeedControlPeriod++;//速度环控制周期计算量自动加1
if(g_nMainEventCount>=5)//SysTick是1ms一次，这里判断语句大于5就是5ms运行一次
{
g_nMainEventCount=0;//主事件循环每5ms循环一次，这里清零，重新计时。
GetMotorPulse(); //每5ms捕获一次脉冲
}else if(g_nMainEventCount==1){//这1ms时间片段获取数据和角度计算
GetMpuData();//获取MPU-6050数据
AngleCalculate(); //进行角度计算
}else if(g_nMainEventCount==2){
AngleControl(); //这1ms时间片段进行角度控制
}else if(g_nMainEventCount==3){
g_nSpeedControlCount++;
if(g_nSpeedControlCount >= 5)
{
SpeedControl(); //速度控制，25ms进行一次
g_nSpeedControlCount=0; //清零
g_nSpeedControlPeriod=0;//清零
}
}else if(g_nMainEventCount==4){
MotorOutput(); //电机输出函数，每5ms执行一次
}
ButtonScan();
/* USER CODE END SysTick_IRQn 0 */
HAL_IncTick();
/* USER CODE BEGIN SysTick_IRQn 1 */
HAL_SYSTICK_IRQHandler(); //这句要加上的
/* USER CODE END SysTick_IRQn 1 */
}
主要是在第 3 个 1ms 时间片段里增加了速度控制代码 SpeedControl();。值得注意的是，我们在这里用了一个计数变量 g_nSpeedControlCount 在时间片段里进行计数，要知道，每个 1ms 时间片段是每 5ms 才运行一次，我们设置 g_nSpeedControlCount >= 5 就运行速度控制，并且清零，这就是 5*5=25ms 才进行一次速度控制。
为什么设置 25ms 才进行一次速度控制，而不是更大或更小的数值？这个值太大会降低速度环控制频率，太小会导致速度的误差大，所以要适中，25ms 是一个实验数值，经过多次实验验证该数值的效果较好，所以采用 25ms。当然，也可以尝试其他的控制频率。
确定 fP 与 fI 值的极性（即g_fSpeed_P和g_fSpeed_I）
另外要说明的是，虽然这里的 PI 控制器是速度控制常用的一种控制器，但是和普通的调速系统不一样，在平衡小车这里的速度控制是正反馈的， 当小车以一定的速度运行的时候，我们要让小车停下来，小车需要行驶更快的速度去“追”，小车运行的速度越快，去“追”的速度也就越快，所以这是一个正反馈的效果。如果使用常规的速度负反馈，当小车以一定的速度运行的时候，我们通过减速让小车慢下来，小车会因为惯性向前倒下。
下面介绍一种确定速度控制是正反馈还是负反馈的方法。根据之前的估计，先把角度环的两个PD参数设成0，设定速度环两个参数 g_fSpeed_P=-10，g_fSpeed_I=g_fSpeed_P/200（写代码时把这个值算出来）。当我们拿起小车，旋转其中一个小车轮胎的时候，根据我们设定的速度偏差 fDelta = CAR_SPEED_SET-g_fCarSpeed；另外一个车轮会反向转动，让偏差趋向于零。这就是常规的速度控制里面的负反馈，不是我们需要的效果。接下来设定 g_fSpeed_P=10，g_fSpeed_I=g_fSpeed_P/200（写代码时把这个值算出来）。此时，当我们旋转其中一个小车轮胎的时候，两个轮胎会往相同的方向加速，直至电机的最大速度，这是典型的正反馈效果，也是我们期望看到的。至此，我们可以确定 kp、ki 的符号应该是正的。
确定 fP 与 fI 的大小（开启直立控制）
下面我们进行平衡小车速度控制 fP 与 fI 值的整定，此时需要在直立环调好的基础上进行，因为我们需要结合直立环观察速度环对直立环的影响。
在调试的过程中设定速度控制的目标为零，所以，调试的理想结果应该是：小车保持平衡的同时，速度接近于零。实际上，因为小车存在比较大的转动惯量和惯性，并且齿轮减速器存在死区，很难调试到让小车完全保持静止的，我们调试两轮自平衡小车只是为了学习 PID 控制算法，所以，没有必要花太多的时间去调参数，让小车完全静止，只要能够大概实现我们需要的功能，并在这个过程对 PID 有进一步的了解即可。
先调试 fI 的大小
确定参数的原则是：
fI 一直增加，直到出现大幅度的低频来回摆动。
设定 fI = 0.01，这个时候我们可以看到，小车基本没有什么改变。
设定 fI = 0.05，这个时候我们可以看到，小车好像有一点来回摆动的趋势。
设定 fI = 0.15，这个时候我们可以看到，小车已经有非常明显的大幅度来回摆动。这组参数已经过大了，我们可以得到 0.15 为临界值，我们取比 0.15 小一点的值就可以，比如 0.1。
再调试 fP 的大小（fI暂取0.1）
fP 一直增加，直到小车出现原地来回晃动。
设定 fP = 1.0，这个时候我们可以看到，小车基本没有什么改变。
设定 fP = 10.0，这个时候我们可以看到，小车已经没有摆动得那么厉害了，小车的速度控制的响应有所加快，但是来回摆动还是有点大，还是不足以让小车保持接近于静止的状态。。
设定 fP = 20.0，这个时候我们可以看到，已经抵消 fP 带来的来回摆动，小车已经基本没有摆动了，能在原地平衡了，而且用手推一下小车，会发现小车已经有一定的抵抗力，不会轻易就被推倒了，性能很不错。我们接下来尝试加大 fP 值看一下效果。
设定 fP = 30.0，这个时候我们可以看到，小车虽然回正力度增大了，而且响应更加快了，但是稍微加入一点的干扰都会让小车大幅度摆动，抗干扰能力明显不足，所以这个参数不可取。
微调参数
至此，我们已得到 fP=20.0，fI=0.1 是速度控制 P、 I 参数的可取值，但这组参数并非最优值，不只是这组参数可以让两轮自平衡小车稳定平衡，我们的电机性能比较好，可取范围比较大，用户可以自行在这组参数范围内进行微调，以获得更好的性能。
我们再来体验一下速度控制负反馈在平衡小车里面的效果，设定 fP=-20.0，fI=-0.1，这个时候我们可以看到， 小车会迅速往一个方向倒下。也就是说常规的速度负反馈在我们这边是“帮倒忙” 了！
速度环参数暂时整定完，在保持速度环参数不变的基础上，我们可以适当增大角度环的PD参数，提升小车整体的抗干扰能力，经过微调，反复试验，可以得到效果更好的的PD参数，如下图所示：
float g_fAngle_P = 70.0; //角度环P参数
float g_fAngle_D = 2.3;//角度环D参数
float g_fSpeed_P = 20.25;//速度环P参数
float g_fSpeed_I = 0.108;//速度环I参数
至此，速度控制调试部分就告一段落了。