本小节主要教你编写角度环 PID 控制相关的代码和对 PID 参数进行整定。
补充相关初始化代码为了降低学习难度，便于萌新们学习，在编码器和驱动电机的例程中，我们以一个电机为例，进行了具体的操作设置。但是，两轮自平衡小车是有两个电机的，所以，在进行直立平衡角度环 PID 控制之前，我们需要先对另一个电机相关的还没初始化的相关引脚进行初始化。
在《PWM与TB6612FNG驱动电机》一节中，我们配置了一个电机的相关控制引脚（TIM3_CH2、PA3、PA4），在这里对另一个电机的相关控制引脚（TIM3_CH1、PB0、PB1）。
回到 STM32CubeMX 软件界面，在左侧 Pinout&Configuration 界面中的 Timers 下拉中点击 TIM3，然后在 TIM3 Mode and Configuration 的 Mode 中将 Channel1 选择为 PWM Generation CH1。因为我们之前配置 Channel2 的时候，已经对参数设置选项卡做出了对应的修改，这时就不用再修改了。
回到 STM32CubeMX 软件界面，在右侧界面的芯片中分别点击 PB0、PB1，并将其配置为 GPIO_Output。在 System Core 下拉菜单中选择 GPIO，然后在左侧的 System Core 下拉菜单中选择 GPIO，然后在 GPIO Mode and Configuration 中对 PA3、PA4 引脚进行配置，GPIO output level 代表 GPIO 默认输出电平，在这里设置为低电平；GPIO mode 代表 GPIO 引脚模式，在这里设置为推挽输出；GPIO Pull-up/Pull-down 即 GPIO 上拉或下拉，在这里设置为既不上拉也不下拉；Maximum output speed 即 最大输出速度，在这里设置为低速；User Label 即用户标签，在这里将 PA0 改为 AIN1，PB1 改为 AIN2。
在《Timer编码器模式读取编码器》一节中，我们只配置 TIM4 为编码器模式，没有配置 TIM2。在这里对 TIM2 进行补充配置，其实很简单，跟之前配置 TIM4 一样的。
进入我们上一小节修改过的 MiaowLabs-Demo 文件夹，找到 MiaowLabs-Demo.ioc 工程文件，双击，打开工程。在左侧 Pinout&Configuration 界面中的 Timers 下拉中点击 TIM2，然后在 TIM2 Mode and Configuration 的 Mode 中将 Combined Channels 选择为 Encoder Mode，即编码器模式。
在 Configuration 中选择 Parameter Setting 选项卡，进行基本参数配置。其中，Counter Mode 默认为 Up，即向上计数。Counter Period 设置为 65535，即计数器周期，这是一个 16 位的自动加载寄存器，填写范围为 0~65535。Encoder Mode 设置为 Encoder Mode TI1 and TI2，即两个输入 TI1 和 TI2 都被用来作为增量编码器的接口。Polarity 默认为 Rising Edge，即为捕获上升沿。其他参数默认即可。
点击 GENERATE CODE，重新生成代码。
打开 MDK-ARM 工程，左侧 Application/User main.c 源文件的 main 函数里的 GPIO 初始化函数： MX_GPIO_Init();，可以看到里面新增了我们刚才添加的 AIN1 和 AIN2 引脚。
点开 TIM3 初始化函数 MX_TIM3_Init();，能看到里面 Channel1 和 Channel2 的相关代码都已经有了。
至此，STM32CubeMX 软件部分的初始化配置就完成了，接下来，要修改我们之前定义的算法代码。
角度环PID控制在 control.c 文件中里敲下以下代码：
#include "control.h"#include "filter.h"#include "mpu6050.h"#include "math.h"#include "outputdata.h"#include "tim.h"#include "main.h"#define MOTOR_OUT_DEAD_VAL 0 //死区值#define MOTOR_OUT_MAX 100 //占空比正最大值#define MOTOR_OUT_MIN (-100) //占空比负最大值#define CAR_ANGLE_SET 0//目标角度#define CAR_ANGLE_SPEED_SET 0//目标角速度short x_nAcc,y_nAcc,z_nAcc;//加速度x轴、y轴、z轴数据short x_nGyro,y_nGyro,z_nGyro;//陀螺仪x轴、y轴、z轴数据float x_fAcc,y_fAcc,z_fAcc;//用于存储加速度x轴、y轴、z轴数据运算后的数据float g_fAccAngle;//加速度传感器经过atan2()解算得到的角度float g_fGyroAngleSpeed;//陀螺仪角速度float g_fCarAngle;//小车倾角float dt = 0.005;//互补滤波器控制周期unsigned int g_nMainEventCount;//主事件计数，用在中断中unsigned int g_nGetPulseCount;//捕获脉冲计数，用在中断中unsigned int g_nLeftMotorPulse,g_nRightMotorPulse;//全局变量，保存左电机脉冲数值int nPwmBais;//PWM增量int nLeftMotorPwm,nRightMotorPwm;//左电机PWM输出总量，左电机PWM输出总量int nLeftMotorErrorPrev,nRightMotorErrorPrev;//左电机上一次偏差，右电机上一次偏差float g_fLeftMotorOut,g_fRightMotorOut;float g_fAngleControlOut;void GetMpuData(void)//读取MPU-6050数据{ MPU_Get_Accelerometer(&x_nAcc,&y_nAcc,&z_nAcc);//获取MPU6050加速度数据 MPU_Get_Gyroscope(&x_nGyro,&y_nGyro,&z_nGyro); //获取MPU6050陀螺仪数据}void AngleCalculate(void)//角度计算{ //-------加速度数据处理-------------------------- //量程为±2g时，灵敏度：16384 LSB/g x_fAcc = x_nAcc / 16384.0; y_fAcc = y_nAcc / 16384.0; z_fAcc = z_nAcc / 16384.0; g_fAccAngle = atan2(y_fAcc,z_fAcc) * 180.0 / 3.14; //-------陀螺仪数据处理------------------------- //范围为2000deg/s时，换算关系：16.4 LSB/(deg/s) g_fGyroAngleSpeed = x_nGyro / 16.4; //计算角速度值 //-------互补滤波--------------- g_fCarAngle = ComplementaryFilter(g_fAccAngle, g_fGyroAngleSpeed, dt); OutData[0]=g_fAccAngle;//发送加速度初步计算的角度 OutData[1]=g_fGyroAngleSpeed;//发送陀螺仪角速度 OutData[2]=g_fCarAngle;//发送数据融合得到的角度}void GetMotorPulse(void)//读取电机脉冲{ g_nLeftMotorPulse = (short)(__HAL_TIM_GET_COUNTER(&htim4));//获取计数器值 __HAL_TIM_SET_COUNTER(&htim4,0);//TIM4计数器清零 g_nRightMotorPulse = (short)(__HAL_TIM_GET_COUNTER(&htim2));//获取计数器值 __HAL_TIM_SET_COUNTER(&htim2,0);//TIM2计数器清零}int SpeedInnerControl(int nPulse, int nTarget, int nPwm, int nErrorPrev)//速度内环控制{ int nError;//偏差 float fP = 10.0, fI = 0.9;//这里只用到PI，参数由电机的种类和负载决定 nError = nPulse - nTarget;//偏差 = 目标速度 - 实际速度 nPwmBais = fP * (nError - nErrorPrev) + fI * nError;//增量式PI控制器 nErrorPrev = nError;//保存上一次偏差 nPwm += nPwmBais;//增量输出 if(nPwm > 100) nPwm = 100;//输出饱和处理，限制上限，防止超出PWM量程 if(nPwm