运动速度闭环外环PID控制

在 control.c 文件中里新增以下定义：

#define CAR_SPEED_SET 0//小车目标速度
#define CAR_POSITION_MAX 200
#define CAR_POSITION_MIN (-200)
float g_fCarSpeed;//小车实际速度
float g_fCarSpeedPrev;//小车前一次速度
float g_fCarPosition;//小车路程
long g_lLeftMotorPulseSigma;//左电机25ms内累计脉冲总和
long g_lRightMotorPulseSigma;//右电机25ms内累计脉冲总和
float g_fSpeedControlOut;//速度环输出

新增速度环控制函数代码：

void SpeedControl(void)//速度外环控制函数
{
      float fP=0.6,fI=0.03; //速度环PI参数，I取0.1时，小车出现明显来回摆动。      
    float fDelta;//临时变量，用于存储误差

    g_fCarSpeed = (g_lLeftMotorPulseSigma + g_lRightMotorPulseSigma ) / 2;//左轮和右轮的速度平均值等于小车速度
    g_lLeftMotorPulseSigma = g_lRightMotorPulseSigma = 0;      //全局变量，注意及时清零

    g_fCarSpeed = 0.7 * g_fCarSpeedPrev + 0.3 * g_fCarSpeed ;//低通滤波，使速度更平滑
    g_fCarSpeedPrev = g_fCarSpeed; //保存前一次速度  

    fDelta = CAR_SPEED_SET - g_fCarSpeed;//误差=目标速度-实际速度  
    g_fCarPosition += fDelta;//对速度误差进行积分   

    //设置积分上限设限
    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_fSpeedControlOut = fDelta * fP + g_fCarPosition * fI; //PI控制器，输出=误差*P+误差积分*I 
}

修改电机输出函数，加入速度环控制量：

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;

声明外部用到的变量。

void SpeedControl(void);

声明变量，等下中断里要调用。

在 stm32fxx_it.c 中更新以下代码：

void SysTick_Handler(void)
{
  /* USER CODE BEGIN SysTick_IRQn 0 */

  g_nMainEventCount++;//每进一次中断，主事件函数自动加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; //清零
        }

    }else if(g_nMainEventCount==4){    
        MotorOutput();         //电机输出函数，每5ms执行一次
    }
    ButtonScan();
  /* USER CODE END SysTick_IRQn 0 */
  HAL_IncTick();
  /* USER CODE BEGIN SysTick_IRQn 1 */

  /* USER CODE END SysTick_IRQn 1 */
}

主要是在第 3 个 1ms 时间片段里增加了速度控制代码 SpeedControl();。值得注意的是，我们在这里用了一个计数变量 g_nSpeedControlCount 在时间片段里进行计数，要知道，每个 1ms 时间片段是每 5ms 才运行一次，我们设置 g_nSpeedControlCount >= 5 就运行速度控制，并且清零，这就是 5*5=25ms 才进行一次速度控制。

为什么设置 25ms 才进行一次速度控制，而不是更大或更小的数值？这个值太大会降低速度环控制频率，太小会导致速度的误差大，所以要适中，25ms 是一个实验数值，经过多次实验验证该数值的效果好，所以采用 25ms。

确定 fP 与 fI 值的极性

另外要说明的是，虽然这里的PI控制器是速度控制常用的一种控制器，但是和普通的调速系统不一样，这里的速度控制是正反馈的，当小车以一定的速度运行的时候，我们要让小车停下来，小车需要行驶更快的速度去“追”，小车运行的速度越快，去“追”的速度也就越快，所以这是一个正反馈的效果。如果使用常规的速度负反馈，当小车以一定的速度运行的时候，我们通过减速让小车慢下来，小车会因为惯性向前倒下。

下面介绍一种确定速度控制是正反馈还是负反馈的方法。根据之前的估计，先设定 fP=-0.1，fI=fP/20。当我们拿起小车，旋转其中一个小车轮胎的时候，根据我们设定的速度偏差 fDelta = CAR_SPEED_SET-g_fCarSpeed；另外一个车轮会反向转动，让偏差趋向于零。这就是常规的速度控制里面的负反馈，不是我们需要的效果。接下来设定 kp=0.1，ki=kp/20。此时，当我们旋转其中一个小车轮胎的时候，两个轮胎会往相同的方向加速，直至电机的最大速度，这是典型的正反馈效果，也是我们期望看到的。至此，我们可以确定 kp、ki 的符号应该是正的。

确定 fP 与 fI 的大小（开启直立控制）

下面我们进行平衡小车速度控制 fP 与 fI 值的整定，此时需要在直立环调好的基础上进行，因为我们需要结合直立环观察速度环对直立环的影响。

在调试的过程中设定速度控制的目标为零，所以，调试的理想结果应该是：小车保持平衡的同时，速度接近于零。实际上，因为小车存在比较大的转动惯量和惯性，并且齿轮减速器存在死区，很难调试到让小车完全保持静止的，我们调试两轮自平衡小车只是为了学习 PID 控制算法，所以，没有必要花太多的时间去调参数，让小车完全静止，只要能够大概实现我们需要的功能，并在这个过程对 PID 有进一步的了解即可。

先调试 fI 的大小

确定参数的原则是：

fI 一直增加，直到出现大幅度的低频来回摆动。

设定 fI = 0.01，这个时候我们可以看到，小车基本没有什么改变。
设定 fI = 0.05，这个时候我们可以看到，小车好像有一点来回摆动的趋势。
设定 fI = 0.1，这个时候我们可以看到，小车已经有非常明显的大幅度来回摆动。这组参数已经过大了，我们可以得到 0.1 为临界值，我们取比 0.1 小一点的值就可以，比如 0.06。

再调试 fP 的大小

fP 一直增加，直到小车出现原地来回晃动。

设定 fP = 0.1，这个时候我们可以看到，小车基本没有什么改变。
设定 fP = 0.3，这个时候我们可以看到，小车已经没有摆动得那么厉害了，小车的速度控制的响应有所加快，但是来回摆动还是有点大，还是不足以让小车保持接近于静止的状态。。
设定 fP = 0.6，这个时候我们可以看到，已经抵消 fP 带来的来回摆动，小车已经基本没有摆动了，能在原地平衡了，而且用手推一下小车，会发现小车已经有一定的抵抗力，不会轻易就被推倒了，性能很不错。我们接下来尝试加大 fP 值看一下效果。
设定 fP = 1.2，这个时候我们可以看到，小车虽然回正力度增大了，而且响应更加快了，但是稍微加入一点的干扰都会让小车大幅度摆动，抗干扰能力明显不足，所以这个参数不可取。

微调参数

至此，我们已得到 fP=0.6，fI=0.0.05 是速度控制 P、 I 参数的可取值，但这组参数并非最优值，不只是这组参数可以让两轮自平衡小车稳定平衡，我们的电机性能比较好，可取范围比较大，用户可以自行在这组参数范围内进行微调，以获得更好的性能。

我们再来体验一下速度控制负反馈在平衡小车里面的效果，设定 fP=-0.6，fI=-0.1，这个时候我们可以看到，小车会迅速往一个方向倒下。也就是说常规的速度负反馈在我们这边是“帮倒忙” 了！

至此，速度控制调试部分就告一段落了。