提高PWM频率为24kHz，控制范围为0~1000

经过上面的一系列实验下来，我们已经初步调好了两轮自平衡小车的平衡功能。但是，不知道你们有没有发现两个问题：

  1. 在小车运行时，电机会发出“滋滋滋滋”的声音，长时间听到这个声音可能会感觉到烦躁。这是因为我们之前设置的 PWM 频率为 10kHz，而人耳能感受到的振动频率范围约为20Hz~20kHz。

  2. 小车的速度环输出量如果过大，会干扰到角度环。如果速度环能比较平滑地输出，那就不会干扰到角度环，小车就能够更加稳定。

提高 PWM 频率为 24kHz

既然 10kHz 是人耳能听到的工作频率，那么我们可以尝试提高 PWM 频率来解决这个问题。

进入我们上一小节实验的 MiaowLabs-Demo 文件夹，找到 MiaowLabs-Demo.ioc 工程文件，双击，打开工程。在 Pinout&Configuration 界面中的 Timers 里点击 TIM3，在 Parameter Settings 选项卡里将 Prescaler 改成 3，Counter Period 改成 1000。此时，PWM = 72000000 / 3 / 1000 = 24000 = 24kHz。

修改PWM频率
Image 5.18.1 - 修改PWM频率

记得，先把我们文件夹里的中文和空格全部删掉，比如将 MiaowLabs-Demo - 13、提高PWM频率为24k，控制范围为0至1000 删掉中文变成 MiaowLabs-Demo再重新生成代码。不然，会提示以下错误：

目录下有中文和空格时会提示类似的错误
Image 5.18.2 - 目录下有中文和空格时会提示类似的错误

我们进入 MiaowLabs-Demo/MDK-ARM 文件夹，双击 MiaowLabs-Demo.uvprojx 文件，使用 MDK-ARM 打开工程。

我们打开 main.c 文件，在 main() 主函数中找到 MX_TIM3_Init()，右键点击 Go to Defition of "MX_TIM3_Init();"，转跳到该函数。

转跳到该函数
Image 5.18.3 - 转跳到该函数

可以看到 TIM3 的初始化已经重新生成我们设定的代码。

TIM3初始化代码已经重新生成
Image 5.18.4 - TIM3初始化代码已经重新生成

这样一来，我们 PWM 的可控范围不再是 0~100，而是变成了 0~1000。也就是说，当设为 PWM 设为 1000 时，占空比才是 100%。

我们之前的代码也要做出对应的改动。主要是输出上下限和 PID 参数的改动。

在 control.c 里，修改代码：

#define MOTOR_OUT_MAX           1000    //占空比正最大值
#define MOTOR_OUT_MIN         (-1000)   //占空比负最大值

#define CAR_POSITION_MAX 900//路程（速度积分）上限
#define CAR_POSITION_MIN (-900)//路程（速度积分）下限

然后，就是重新整定 PID 参数。这里先不进行整定，接着把速度平滑输出函数加进去。

速度平滑输出

先在 control.c 中添加几个变量、宏定义：

#define SPEED_CONTROL_PERIOD    25      //速度环控制周期
float g_fSpeedControlOut,g_fSpeedControlOutNew,g_fSpeedControlOutOld;//速度环输出
int g_nSpeedControlPeriod;//速度环控制周期计算量

对速度外环控制函数 SpeedControl() 进行改造，并编写速度外环平滑输出函数 SpeedControlOutput()。

void SpeedControl(void)//速度外环控制函数
{
      float fP=10.25,fI=0.108; //速度环PI参数，    
    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_fSpeedControlOutOld = g_fSpeedControlOutNew;//保存上一次输出

    g_fSpeedControlOutNew = fDelta * fP + g_fCarPosition * fI; //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 值全部加完刚好是 100ms，当然要想形成每 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);
}

并且在 control.h 头文件中，进行声明。

extern int g_nSpeedControlPeriod;//速度环控制周期计算量
void SpeedControlOutput(void);

回到 stm32f1xx_it.c 文件中，找到滴答中断服务函数 SysTick_Handler()，进行修改。

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

  g_nMainEventCount++;//每进一次中断，主事件函数自动加1

  g_nSpeedControlPeriod++;//速度环控制周期计算量自动加1
  SpeedControlOutput(); //速度环控制平滑输出处理，速度的pwm改变量如果在25ms时刻计算出后立刻输出，会造成不平滑抖动等，这段代码就是把这个25ms周期计算一次得到的pwm分配到5个5ms时间去输出，平滑地逐步逼近输出最后的计算值！ 
  if(g_nMainEventCount>=5)//SysTick是1ms一次，这里判断语句大于5就是5ms运行一次
    {
        g_nMainEventCount=0;//主事件循环每5ms循环一次，这里清零，重新计时。    
        GetMotorPulse();
    }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 */

  /* USER CODE END SysTick_IRQn 1 */
}

此时，速度环控制平滑输出函数也编写好了。接着，按照上一个章节调试 PID 的步骤，重新对角度环、速度环的 PID 参数进行整定，这里不再重复说明。经过我们的调试，得到角度环 fP=65.0;fD=2.3 ，速度环 fP=10.25,fI=0.108时，可以取得良好的效果。当然，这不是最优值，萌新们可以在这个范围上下调试，以获得更好的控制效果。