说明：本文章适用于STM32初学者，想完成一个好玩且有深度的项目但不知道从何下手的同学。
平衡车是我入门STM32的第一个实战项目，前前后后和我搭硬件的队友路总（硬件大佬，专注于PCB画板）搭了有七八版。从第一版V1.0手焊版到嘉立创PCB打板到最终的无线充电平衡车，我们碰到了几乎所有可能出现的问题，熬了几天夜硬着头皮解决，功能也是一项一项的添加，最终在学校的电赛选拔赛上拿到了一等奖，也是不负众望。毕竟经历过，所以写这篇文章的目的是让大家少走弯路，能够快速的、有目的性的做出属于自己的那个DIY平衡车，从实战中学习到知识才是最有效率最能检验你对知识的掌握程度。所以跟着我，三天让车站起来，不是梦！
我们在写代码之前一定要想好自己需要写什么，目的是什么，写之后的效果是什么，并且要做好分类，按照编写代码的规则一步一步添加，调用函数。这样我们就算遇到问题也可以很好的排查出是哪里的问题。所以第一步我们列出主控STM32所要实现的功能！
软件需求
-
pwm.c
输出两路PWM波（驱动电机） 高级定时器TIM1 CH1/CH4----------PA8/PA11
连接TB6612的PWMA/PWMB -
motor.c
电机旋转方向位初始化
电机1——PB12/PB13 ——TB6612 AIN1/AIN2
电机2——PB14/PB15 ——TB6612 BIN1/BIN2
PWM赋值函数
PWM限幅
绝对值函数 -
encoder.c
编码器模式初始化
输入捕获模式配置
读取速度函数
因为有两个车轮所以我们开启两个定时器通过定时器输入捕获功能分别读取两轮的速度，这里我们用到的是TIM2/TIM4
编码器1——PA0/PA1——TIM2 A相/B相
编码器2——PB6/PB7——TIM4 A相/B相 -
oled.c
显示函数 -
usart3.c
蓝牙串口配置
TX——PB10
RX——PB11
串口中断接收数据 USART3_IRQHandler（）——控制小车状态 -
control.c
PID函数
直立环Vertical
速度环Velocity
转向环Turn
中断函数（10ms）——PID计算，限幅，加载到电机 -
exti.c
中断引脚初始化——PB5 -
mpu6050建议在mpu6050的例程上进行其他的程序编写，方便高效
移植正点原子的文件就ok其中包括
mpu6050.h
mpuiic.h
inv_mpu.h
inv_mpu_dmp_motion_driver.h
将这些复制到我们的代码工程文件夹中，在keil中添加.c以及包含.h的文件夹路径。
MPU6050 INT——PB5(中断引脚)
IIC SCL/SDA—— PB3/PB4
软件所需的部分就这么多，我们只有全部配置好才能进行下一步的PID调参！
PID算法
PID控制
PID控制，就是对偏差进行比例、积分和微分的控制。
PID由3个单元组成，分别是比例（P）单元、积分（I）单元、微分（D）单位。
工程中P必然存在，在P的基础上又有如PD控制器、PI控制器、PID控制器等。
在pid算法中其实是有一个时间常数存在，我们得P,I,D项是由时间常数等构成的，为了简便我们通常选取PID的运算时间为5ms，10ms，20ms等就直接得出了KP,KI,KD。我们调参时只用对KP,KI,KD的参数进行改变即可。
比例项P：提高响应速度，减小静差。直观作用
**积分项I：**消除稳态误差。只要有偏差，我就积分，有一丁点偏差，我也会积分。积积，就会非常大。直到你偏差变为0.
**微分项D：**减小震荡以及超调。具有预测性，预测未来。风力摆系统中我们要有良好的跟随性，我们的D就要非常大。
位置式PID
适用于开关式闭环，例如温控系统，平衡小车直立环。超了就减，没到就加。一直开关开关以尽可能的达到期望的目标。
在平衡小车系统中：
1.理论分析
位置闭环控制就是根据编码器的脉冲累加测量电机的位置信息，并与目标值进行比较，得到控制偏差，
然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于零的过程。
2.结构框图
/**************************************************************************
函数功能：位置式PID控制器
入口参数：测量值，目标值
返回 值：输出temp
根据位置式离散PID公式
pwm=Kp*e(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]
e(k)代表本次偏差
e(k-1)代表上一次的偏差
∑e(k)代表e(k)以及之前的偏差的累积和;其中k为1,2,,k;
pwm代表输出
**************************************************************************/
int Position_PID (int Nowpoint,int Target)
{
float Position_KP=80,Position_KI=0.1,Position_KD=500;
static float Bias,temp,Integral_bias,Last_Bias;
Bias=Nowpoint-Target; //求出偏差，由测量值减去目标值。
Integral_bias+=Bias; //求出偏差的积分
temp=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias); //位置式PID控制器
Last_Bias=Bias; //保存上一次偏差
return temp; //增量输出
}
总结：Kp误差+Ki误差的积分（程序中积分为累加）+Kd*相邻两次误差的偏差
强调：在调参时要先确定Kp的值在确定Ki及Kd。
增量式PID
适用于速度闭环，电流闭环等，受到偶然误差的影响较小，适用于连续系统。
/**************************************************************************
函数功能：增量PI控制器
入口参数：测量值，目标值
返回 值：输出temp
根据增量式离散PID公式
temp+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)代表本次偏差
e(k-1)代表上一次的偏差 以此类推
temp代表增量输出
temp+=Kp[e（k）-e(k-1)]+Ki*e(k)
**************************************************************************/
int Incremental_PID (int Nowpoint,int Target)
{
float Kp=20,Ki=30,Kd=10;
static int Bias,temp,Last_bias,Prev_bias; //相关内部变量的定义。
Bias=Nowpoint-Target; //求出偏差，由测量值减去目标值。
temp+=Kp*(Bias-Last_bias)+Ki*Bias+Kd*(Bias-2*Last_bias+Prev_bias); //使用增量 PI 控制器
Prev_bias=Last_bias; //保存上上一次偏差
Last_bias=Bias; //保存上一次偏差
return temp; //增量输出
}
总结：Kp相邻两次误差的偏差+Ki误差+Kd*（误差-2倍的上次偏差+上上次偏差）
强调：在调参时要先确定Ki的值在确定Kp及Kd。（本人踩过的大坑！！！直接乘以误差的那一项系数对系统影响最大！！！）
程序编写
按照上述的软件需求我们来一一编写对应的代码。切忌不要急于求成，一部分一部分调通在加入新的部分，确保不会因为一些小细节而出大问题。本人很喜欢平衡小车之家、正点原子的代码风格，也学习了平衡小车之家公司的教程以及b站大佬的教程，综合了以上的学习，才写出了以下的代码，以及培养了一个写代码的风格。作为新手我们要多学习多借鉴，在巨人的肩膀上前行，这样我们才能进步更快！
一、pwm.c
PWM波的输出应该非常简单，主要就是管脚的初始化，定时器pwm模式的初始化，输出比较模式等等，这里我们直接上代码！
我们这里调用函数PWM_Init_TIM1(0,7199);
0分频，周期为7199 也就是PWM波的频率为10K
void PWM_Init_TIM1(u16 Psc,u16 Per)
{
GPIO_InitTypeDef GPIO_InitStruct;
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_OCInitTypeDef TIM_OCInitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_TIM1 | RCC_APB2Periph_AFIO,ENABLE);//开启时钟
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_AF_PP;//初始化GPIO--PA8、PA11为复用推挽输出
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_8 |GPIO_Pin_11;
GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStruct);
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//初始化定时器。
TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;
TIM_TimeBaseInitStruct.TIM_Period=Per;
TIM_TimeBaseInitStruct.TIM_Prescaler=Psc;
TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct);/*【2】*///TIM2
TIM_OCInitStruct.TIM_OCMode=TIM_OCMode_PWM1;//初始化输出比较
TIM_OCInitStruct.TIM_OCPolarity=TIM_OCPolarity_High;
TIM_OCInitStruct.TIM_OutputState=TIM_OutputState_Enable;
TIM_OCInitStruct.TIM_Pulse=0;
TIM_OC1Init(TIM1,&TIM_OCInitStruct);
TIM_OC4Init(TIM1,&TIM_OCInitStruct);
TIM_CtrlPWMOutputs(TIM1,ENABLE);//高级定时器专属--MOE主输出使能
TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable);/*【3】*///ENABLE//OC1预装载寄存器使能
TIM_OC4PreloadConfig(TIM1,TIM_OCPreload_Enable);//ENABLE//OC4预装载寄存器使能
TIM_ARRPreloadConfig(TIM1,ENABLE);//TIM1在ARR上预装载寄存器使能
TIM_Cmd(TIM1,ENABLE);//开定时器。
}
二、motor.c
1.电机旋转方向位初始化
/*电机初始化函数*/
void Motor_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//开启时钟
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_Out_PP;//初始化GPIO--PB12、PB13、PB14、PB15为推挽输出
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_12 |GPIO_Pin_13 |GPIO_Pin_14 |GPIO_Pin_15;
GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOB,&GPIO_InitStruct);
}
2.PWM限幅函数
int PWM_MAX=7200,PWM_MIN=-7200; //PWM限幅变量
/*限幅函数*/
void Limit(int *motoA,int *motoB)
{
if(*motoA>PWM_MAX)*motoA=PWM_MAX;
if(*motoA<PWM_MIN)*motoA=PWM_MIN;
if(*motoB>PWM_MAX)*motoB=PWM_MAX;
if(*motoB<PWM_MIN)*motoB=PWM_MIN;
}
3.绝对值函数
/*绝对值函数*/
int abs(int p)
{
int q;
q=p>0?p:(-p);
return q;
}
4.PWM赋值函数
/*赋值函数*/
/*入口参数：PID运算完成后的最终PWM值*/
void Load(int moto1,int moto2)
{
//1.研究正负号，对应正反转
if(moto1>0) Ain1=1,Ain2=0;//正转
else Ain1=0,Ain2=1;//反转
//2.研究PWM值
TIM_SetCompare1(TIM1,abs(moto1));
if(moto2>0) Bin1=1,Bin2=0;
else Bin1=0,Bin2=1;
TIM_SetCompare4(TIM1,abs(moto2));
}
三、encoder.c
编码器输入捕获
编码器模式配置为TI12模式：在T1和T2的所有边沿计数。以及都不返相。
【正反转】
正转：T1超前T2相位90度。
反转：T1滞后T2相位90度。
【模式】
TI1模式：在T1的所有边沿计数。
TI2模式：在T2的所有边沿计数。
TI12模式：在T1和T2的所有边沿计数。
1.编码器模式初始化
void Encoder_TIM2_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_ICInitTypeDef TIM_ICInitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//开启时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IN_FLOATING;//初始化GPIO--PA0、PA1
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_0 |GPIO_Pin_1;
GPIO_Init(GPIOA,&GPIO_InitStruct);
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//初始化定时器。
TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;
TIM_TimeBaseInitStruct.TIM_Period=65535;
TIM_TimeBaseInitStruct.TIM_Prescaler=0;
TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStruct);
TIM_EncoderInterfaceConfig(TIM2,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//配置编码器模式
TIM_ICStructInit(&TIM_ICInitStruct);//初始化输入捕获
TIM_ICInitStruct.TIM_ICFilter=10;
TIM_ICInit(TIM2,&TIM_ICInitStruct);
TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE);//配置溢出更新中断标志位
TIM_SetCounter(TIM2,0);//清零定时器计数值
TIM_Cmd(TIM2,ENABLE);//开启定时器
}
void Encoder_TIM4_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_ICInitTypeDef TIM_ICInitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IN_FLOATING;
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_6 |GPIO_Pin_7;
GPIO_Init(GPIOB,&GPIO_InitStruct);
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);
TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;
TIM_TimeBaseInitStruct.TIM_Period=65535;
TIM_TimeBaseInitStruct.TIM_Prescaler=0;
TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStruct);
TIM_EncoderInterfaceConfig(TIM4,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
TIM_ICStructInit(&TIM_ICInitStruct);
TIM_ICInitStruct.TIM_ICFilter=10;
TIM_ICInit(TIM4,&TIM_ICInitStruct);
TIM_ClearFlag(TIM4,TIM_FLAG_Update);
TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE);
TIM_SetCounter(TIM4,0);
TIM_Cmd(TIM4,ENABLE);
}
2.读取速度函数
因为我们在10ms中断里去读取速度，时间很短，就不可能会产生溢出，所以我们就以当前的计数值去近似作为他的速度。也就是脉冲个数。作为PID速度环的入口参数。
int Read_Speed(int TIMx)
{
int value_1;
switch(TIMx)
{
case 2:value_1=(short)TIM_GetCounter(TIM2);TIM_SetCounter(TIM2,0);break;//IF是定时器2，1.采集编码器的计数值并保存。2.将定时器的计数值清零。
case 4:value_1=(short)TIM_GetCounter(TIM4);TIM_SetCounter(TIM4,0);break;
default:value_1=0;
}
return value_1;
}
三、oled.c
0.96寸OLED 大家可以根据自己使用的屏幕，去调整所编写的函数。这里写的是显示三个姿态角在屏幕上。对应的.c文件太长了而且网上资源比较多我就不在此赘述。这里只编写了我们使用当中该怎么用。
1.显示函数
u8 string[10] = {0};
sprintf((char *)string,"Pitch:%.2f",Pitch);//0300
OLED_ShowString(0,0,string,8);
sprintf((char *)string,"Roll :%.2f",Roll);//0300
OLED_ShowString(0,2,string,8);
sprintf((char *)string,"Yaw :%.2f",Yaw);//0300
OLED_ShowString(0,4,string,8);
四、usart3.c
蓝牙串口
1.蓝牙串口配置
波特率我们在主函数中设置为115200即
uart3_init(115200);
void uart3_init(u32 bound)
{
//GPIO端口设置
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);//时钟GPIOB、USART3
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);
//USART3_TX PB10
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(GPIOB, &GPIO_InitStructure);
//USART3_RX PB11
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOB, &GPIO_InitStructure);
//USART 初始化设置
USART_InitStructure.USART_BaudRate = bound;//一般设置为9600;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_Init(USART3, &USART_InitStructure);
USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);//开启中断
USART_Cmd(USART3, ENABLE); //使能串口
}
2.串口中断接收数据
大家可以根据需要去删减调整，通常我们会改变标志位例如模式选择标志位或者前进标志位，后退标志位，我们在中断函数中去判断这些标志位，从而达到控制效果或者模式的切换。
void USART3_IRQHandler(void)
{
int Bluetooth_data;
if(USART_GetITStatus(USART3,USART_IT_RXNE)!=RESET)//接收中断标志位拉高
{
Bluetooth_data=USART_ReceiveData(USART3);//保存接收的数据
if(Bluetooth_data==0x00)Fore=0,Back=0,Left=0,Right=0;//刹
else if(Bluetooth_data==0x01)Fore=1,Back=0,Left=0,Right=0;//前
else if(Bluetooth_data==0x05)Fore=0,Back=1,Left=0,Right=0;//后
else if(Bluetooth_data==0x03)Fore=0,Back=0,Left=1,Right=0;//左
else if(Bluetooth_data==0x07)Fore=0,Back=0,Left=0,Right=1;//右
else if(Bluetooth_data==0x09)Mode_flag=1;//蓝牙控制模式
else if(Bluetooth_data==0x11)Mode_flag=2;//循迹控制模式
else if(Bluetooth_data==0x13)Mode_flag=3;//避障模式
else Fore=0,Back=0,Left=0,Right=0,Mode_flag=0;//刹
}
}
五、exti.c
1.中断引脚初始化
这里我们设置为上拉输入，根据MPU6050的使用手册可知当其成功采集到一次数据，它的INT引脚就会拉低，产生下降沿从而触发外部中断。这里中断引脚为PB5。
void MPU6050_EXTI_Init(void)
{
EXTI_InitTypeDef EXTI_InitStruct;
GPIO_InitTypeDef GPIO_InitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO,ENABLE);//开启时钟
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IPU;/**【1】**///GPIO_Mode_AF_PP
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_5;//PB5配置为上拉输入
GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOB,&GPIO_InitStruct);
GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource5);//
EXTI_InitStruct.EXTI_Line=EXTI_Line5;
EXTI_InitStruct.EXTI_LineCmd=ENABLE;
EXTI_InitStruct.EXTI_Mode=EXTI_Mode_Interrupt;
EXTI_InitStruct.EXTI_Trigger=EXTI_Trigger_Falling;
EXTI_Init(&EXTI_InitStruct);
}
六、MPU6050
可以参考正点原子的相关代码，这里只展示应该怎样使用。我们调用其库文件后，直接调用库函数即可。如下所示即可将
角度Pitch，Roll，Yaw
角速度gyrox，gyroy，gyroz
角加速度aacx，aacy，aacz读取出来。
mpu_dmp_get_data(&Pitch,&Roll,&Yaw); //角度
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //陀螺仪
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //加速度
七，control.c(重中之重)
1.PID函数编写
直立环Vertical
小车保持平衡的直接影响环：
理论就是小车往那边倒，车轮就往哪边开，才可以保持车子的平衡。
意思就是如果小车往前倾斜，那我们直立环PID的输出就为一个向前的PWM占空比的值
如果小车往后倾斜，那我们直立环PID的输出就为一个向后的PWM占空比的值在这里为负值。在我们Load函数中进行正负的判断以及车轮方向的控制。
Med为中值角度，就是我们硬件上我们不上电他能够保持平横时间最长所对应的角度。
Angle为我们MPU6050采集到的角度Roll或者是Pitch 大家根据你的陀螺仪安装方向来配置即可。
gyro_X为角度Roll或者是Pitch方向上角速度。
/*********************
直立环PD控制器：Kp*Ek+Kd*Ek_D
入口：期望角度、真实角度、真实角速度
出口：直立环输出
*********************/
int Vertical(float Med,float Angle,float gyro_X)
{
int PWM_out;
PWM_out=Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_X-0);
return PWM_out;
}
速度环Velocity
入口参数为：
Target——期望速度（若为0，小车将稳在原地，我们通过蓝牙去控制此值可以达到移动的效果）
encoder_left——左车轮编码器读取到的速度
encoder_right——右车轮编码器读取到的速度
在速度环中我们对误差进行了一个低通滤波以保证曲线的平稳性。
EnC_Err_Lowout=(1-a)Encoder_Err+aEnC_Err_Lowout_last;//使得波形更加平滑，滤除高频干扰，防止速度突变。
我们的平衡车运用了串级PID控制即直立环作为内环，速度环的输出作为直立环的输入。目的是在小车平衡的前提下，对速度进行控制能够更加的顺滑。小车平衡的效果也能更好更稳定。
串级PID理论框图
速度环输入：1.给定速度。2.速度反馈。
输出：角度值（直立环的期望速度输入）
直立环输入：1.给定角度（速度环输出）。2.角度反馈
输出：PWM（直接控制小车）
/*********************
速度环PI：Kp*Ek+Ki*Ek_S
*********************/
int Velocity(int Target,int encoder_left,int encoder_right)
{
static int Encoder_S,EnC_Err_Lowout_last,PWM_out,Encoder_Err,EnC_Err_Lowout;
float a=0.7;
//1.计算速度偏差
Encoder_Err=((encoder_left+encoder_right)-Target);//舍去误差--我的理解：能够让速度为"0"的角度，就是机械中值。
//2.对速度偏差进行低通滤波
//low_out=(1-a)*Ek+a*low_out_last;
EnC_Err_Lowout=(1-a)*Encoder_Err+a*EnC_Err_Lowout_last;//使得波形更加平滑，滤除高频干扰，防止速度突变。
EnC_Err_Lowout_last=EnC_Err_Lowout;//防止速度过大的影响直立环的正常工作。
//3.对速度偏差积分，积分出位移
Encoder_S+=EnC_Err_Lowout;
//4.积分限幅
Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);
//5.速度环控制输出计算
PWM_out=Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;
return PWM_out;
}
转向环Turn
入口参数：gyro_Z —— Z轴角速度
RC——蓝牙接收的期望转向速度
当我们不转向时 RC=0 这时只有一个Turn_Kd在约束Z轴的角速度。效果是我们用手去旋转小车有一个抵抗的力。这样可以让我们的车在平衡时不打转。
当我们转向时 Turn_Kd=0转向时的约束为0，这时我们给RC赋值若为正输出的PWM值为正，若为负输出pwm值为负。这时我们在最终加载到电机的PWM值左电机减右电机加即可形成差速从而达到转向的效果。
/*********************
转向环：系数*Z轴角速度+系数*遥控数据
*********************/
int Turn(int gyro_Z,int RC)
{
int PWM_out;
//这不是一个严格的PD控制器，Kd针对的是转向的约束，但Kp针对的是遥控的转向。
PWM_out=Turn_Kd*gyro_Z + Turn_Kp*RC;
return PWM_out;
}
中断函数（10ms） 所有代码最核心的部分
void EXTI9_5_IRQHandler(void)
{
int PWM_out;
if(EXTI_GetITStatus(EXTI_Line5)!=0)//一级判定
{
if(PBin(5)==0)//二级判定
{
EXTI_ClearITPendingBit(EXTI_Line5);//清除中断标志位
//1.采集编码器数据&MPU6050角度信息。
Encoder_Left=-Read_Speed(2);//电机是相对安装，刚好相差180度，为了编码器输出极性一致，就需要对其中一个取反。
Encoder_Right=Read_Speed(4);
mpu_dmp_get_data(&Pitch,&Roll,&Yaw); //角度
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //陀螺仪
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //加速度
if(Mode_flag==3) //跟随模式
{
UltrasonicWave_StartMeasure();
if(UltrasonicWave_Distance<=70)Target_Speed=10;
else if(UltrasonicWave_Distance>100&&UltrasonicWave_Distance<=200)Target_Speed=-10;
else Target_Speed=0;
}
if(Mode_flag==2)//循迹模式
Tracking_detection();
else if(Mode_flag==1)//蓝牙模式，通过蓝牙控制标志位达到控制的效果
/*前后*/
{ if((Fore==0)&&(Back==0))Target_Speed=0;//未接受到前进后退指令-->速度清零，稳在原地
if(Fore==1)Target_Speed=-25;// 前进1标志位拉高-->需要前进
if(Back==1)Target_Speed=25;//
Target_Speed=Target_Speed>SPEED_X?SPEED_X:(Target_Speed<-SPEED_X?(-SPEED_X):Target_Speed);//限幅
/*左右*/
if((Left==0)&&(Right==0))Turn_Speed=0;
if(Left==1)Turn_Speed-=50; //左转
if(Right==1)Turn_Speed+=50; //右转
Turn_Speed=Turn_Speed>SPEED_Z?SPEED_Z:(Turn_Speed<-SPEED_Z?(-SPEED_Z):Turn_Speed);//限幅( (20*100) * 100 )
/*转向约束*/
if((Left==0)&&(Right==0))Turn_Kd=-0.7;//若无左右转向指令，则开启转向约束
else if((Left==1)||(Right==1))Turn_Kd=0;//若左右转向指令接收到，则去掉转向约束
}
//2.将数据压入闭环控制中，计算出控制输出量。串级PID
Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); //速度环
Vertical_out=Vertical(Velocity_out+Med_Angle,Roll,gyrox); //直立环
Turn_out=Turn(gyroz,Turn_Speed); //转向环
//转向环
PWM_out=Vertical_out;//最终输出
//3.把控制输出量加载到电机上，完成最终的的控制。
MOTO1=PWM_out-Turn_out;//左电机
MOTO2=PWM_out+Turn_out;//右电机
Limit(&MOTO1,&MOTO2); //PWM限幅
Load(MOTO1,MOTO2); //加载到电机上。
// Stop(&Med_Angle,&Roll);
}
}
}
其中的参数我们统一使用宏定义或者全局变量。for example：
u8 Mode_flag; //模式选择标志位
float Med_Angle=-12; //机械中值。
float Target_Speed=0; //期望速度（俯仰）。---用于控制小车前进后退及其速度。
float Turn_Speed=0; //期望速度（偏航）
float
Vertical_Kp=-630//420*0.6=252 770*0.6=462 -1300*0.6=-780 -870 -1050*0.6
,//直立环KP、KD
Vertical_Kd=-1.85; //1*0.6 2.3*0.6=1.38 -3*0.6=-1.8 -4.2*0.6 -3*0.6
float
Velocity_Kp=0.24,//速度环KP、KI 0.21 0.45
Velocity_Ki=0.0012; 0.0015 0.004
float
Turn_Kd=-0.7,//转向环KP、KD -0.7
Turn_Kp=20;//20
int Vertical_out,Velocity_out,Turn_out;//直立环&速度环&转向环 的输出变量
int Vertical(float Med,float Angle,float gyro_Y);//函数声明
int Velocity(int Target,int encoder_left,int encoder_right);
int Turn(int gyro_Z,int RC);
//void chaoshengbo(void);
#define SPEED_X 30 //俯仰(前后)最大设定速度
#define SPEED_Z 100//偏航(左右)最大设定速度
八、NVIC配置(中断优先级)
我们外部中断PID运算的优先级必须是最高的所以在这里取0,0。
void NVIC_Config(void)
{
NVIC_InitTypeDef NVIC_InitStruct;
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//4级抢占,4级响应。
//外部中断
NVIC_InitStruct.NVIC_IRQChannel=EXTI9_5_IRQn;
NVIC_InitStruct.NVIC_IRQChannelCmd=ENABLE;
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority=0;
NVIC_InitStruct.NVIC_IRQChannelSubPriority=0;
NVIC_Init(&NVIC_InitStruct);
//串口
NVIC_InitStruct.NVIC_IRQChannel=USART1_IRQn;
NVIC_InitStruct.NVIC_IRQChannelCmd=ENABLE;
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority=2;
NVIC_InitStruct.NVIC_IRQChannelSubPriority=2;
NVIC_Init(&NVIC_InitStruct);
//蓝牙
NVIC_InitStruct.NVIC_IRQChannel=USART3_IRQn;
NVIC_InitStruct.NVIC_IRQChannelCmd=ENABLE;
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority=2;
NVIC_InitStruct.NVIC_IRQChannelSubPriority=2;
NVIC_Init(&NVIC_InitStruct);
}
到这里我们的代码全部就写好啦，下一步就是PID的调参。在调参之前一定要确保自己代码的每一部分都不会出现问题。比如陀螺仪是否能读到角度，用屏幕显示或者串口来验证好坏。有的陀螺仪就玄学初始化老不通过。可能是焊接等问题。陀螺仪尽量平放时初始化。不然可能会自检不通过。可以拿串口来看速度读取到的脉冲数，静下心来好好思考一下。众所周知B站可是一个学习的地方，我在刚入门的时候也在B站看教程跟着大佬一步一步调试，在这里感谢一下B站天下行走老哥，以及平衡小车之家，正点原子提供的大量资源和对平衡车的思路大家可以b站看一下他的视频一定会对你帮助很大。作为我们的练手项目，希望我们能够已平衡小车来做一个启蒙作用，能够让我们在以后需要能够用到PID的项目举一反三，解决起来更加顺手这才是主要的！多借鉴大佬的想法，看看别人写代码的方式和巧妙之处认真品味。站在巨人的肩膀上前行！当前时代我们都是CV工程师，我们在CV中要添加自己的东西进去，那么这样就换转化为我们自己的东西。以后遇到同样的问题。我们也能够从容的解决。