平衡小车软件篇
平衡小车硬件篇
目录
一、定时器相关代码
1、TIM2
2、TIM3和TIM4
二、中断相关代码
三、电机相关代码
四、OLED屏幕和MUP6050相关代码
五、PID函数
1、直立环
2、速度环
3、转向环
六、控制函数
七、扩展篇
八、效果展示
说明：本篇文章适用于有点STM32单片机基础，和有相关的硬件基础，并且想做一个小项目的人，可以作为一个参考，当然了没有基础的也可以
如果需要代码的话可以私聊我，供参考
下面就进行我们的代码部分
一、定时器相关代码
我这里使用了三个定时器，分别是TIM2,TIM3,TIM4，定时器2用于产生两路PWM波，用来控制小车的速度，TIM3和TIM4用来对编码器的脉冲进行计数，实现电机的闭环控制。
1、TIM2
#include "pwm.h"
void PWM_Init_TIM2(u16 psc,u16 arr)
{
GPIO_InitTypeDef GPIO_InitPwm; //GPIO初始化
TIM_TimeBaseInitTypeDef TIM_PwmInit; //定时器初始化
TIM_OCInitTypeDef TIM_OcPwminit; //指定定时器输出通道初始化
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //配置GPIO时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
GPIO_InitPwm.GPIO_Mode = GPIO_Mode_AF_PP; //推完复用输出
GPIO_InitPwm.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3;
GPIO_InitPwm.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitPwm);
TIM_PwmInit.TIM_CounterMode = TIM_CounterMode_Up;
TIM_PwmInit.TIM_Period = arr;
TIM_PwmInit.TIM_Prescaler = psc;
TIM_PwmInit.TIM_ClockDivision = TIM_CKD_DIV1; //1分频
TIM_TimeBaseInit(TIM2, &TIM_PwmInit);
TIM_OcPwminit.TIM_OCMode = TIM_OCMode_PWM1 ; //比较输出模式pwm1
TIM_OcPwminit.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OcPwminit.TIM_OCNPolarity = TIM_OCNPolarity_Low; //互补输出极性、低电平有效
TIM_OC3Init(TIM2, &TIM_OcPwminit); //初始化通道
TIM_OC4Init(TIM2, &TIM_OcPwminit); //初始化通道
TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);
TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable); //控制波形是立即生效还是定时器发生下一次更新事件时被更新的
//Enable:下一次更新事件时被更新
//Disable:立即生效
TIM_Cmd(TIM2, ENABLE); //使能TIM3的外设
}
由代码可知PWM由定时器2的通道3和通道4输出，对应到单片机引脚图就是PA2和PA3，这里要注意。
2、TIM3和TIM4
TIM3和TIM4配置都差不多，就介绍一个就可以了，定时器配置代码如下
#include "encoder.h"
void Encoder_TIM3_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_ICInitTypeDef TIM_ICInitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//开启时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IN_FLOATING;//初始化
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_6 |GPIO_Pin_7;
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(TIM3,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//配置编码器模式
TIM_ICStructInit(&TIM_ICInitStruct);//初始化输入捕获
TIM_ICInitStruct.TIM_ICFilter=10;
TIM_ICInit(TIM3,&TIM_ICInitStruct);
TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE);//配置溢出更新中断标志位
TIM_SetCounter(TIM3,0);//清零定时器计数值
TIM_Cmd(TIM3,ENABLE);//开启定时器
}
这里要注意有一个配置为编码器模式的函数，函数的具体用法和函数参数什么意思就不在这里解释了，可以去查询相关资料或者后面出文章解释
TIM_EncoderInterfaceConfig(TIM3,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
下面就是定时器中断函数和读取编码器数据的函数了
/*********************
编码器
速度读取函数
入口参数：定时器几
**********************/
int Read_Speed(int TIMx)
{
int value_1;
switch(TIMx)
{ //单位时间内,位移就是速度
case 3:value_1=(short)TIM_GetCounter(TIM3);TIM_SetCounter(TIM3,0);break;
//1.采集编码器的计数值并保存。2.将定时器的计数值清零。
default:value_1=0;
}
return value_1;
}
void TIM3_IRQHandler(void)
{
if(TIM_GetITStatus(TIM3,TIM_IT_Update)!=0){
TIM_ClearITPendingBit(TIM3,TIM_IT_Update);
}
}
速度读取函数中，第一步采集定时器的计数值，也就是编码器的数据，并保存，第二步将定时器清零。
二、中断相关代码
针对中断这部分代码有两种解决方法，也可以用定时器实现，现在就介绍用中断实现方法，其实原理都是一样的。
#include "exti.h"
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;
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);
}
这里配置了一个单片机引脚为外部中断，(PB5)为什么要配置这个引脚呢？这个引脚和MPU6050的中断引脚INT相连，INT是陀螺仪的中断引脚，当采集一次数据时，就会发生一次中断，对于中断后干嘛，在后面说明。
三、电机相关代码
#include "motor.h"
/*电机初始化函数*/
void Motor_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//开启时钟
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_Out_PP;//
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);
}
这个函数也就是配置4个引脚为输出引脚，和电机相连
#include "motor.h"
/*赋值函数*/
void Load(int moto1,int moto2)
{
//数据正负号，对应正反转
if(moto1>0) Ain1=1,Ain2=0;//正转
else Ain1=0,Ain2=1;//反转
//加载PWM值
TIM_SetCompare3(TIM2,GFP_abs(moto1)); //输出
if(moto2>0) Bin1=1,Bin2=0;
else Bin1=0,Bin2=1;
TIM_SetCompare4(TIM2,GFP_abs(moto2));
}
这个函数是用于将PID计算输出的最终值加载到电机上，完成电机的控制。
四、OLED屏幕和MUP6050相关代码
针对这两部分的代码我都是用的官方的示例代码，这个网上都有很多的，陀螺仪只要能读到数据就可以了。
五、PID函数
平衡小车的PID函数由三部分构成，分别为直立环，速度环，转向环三部分构成，直立环就是让小车角度趋近0，速度环就是让电机速度趋近0，转向环就是让电机保持角速度为零。
1、直立环
/*********************
直立环PD控制器：Kp*Ek+Kd*Ek_D
入口：期望角度、真实角度、真实角速度
出口：直立环输出
*********************/
int Vertical(float Med,float Angle,float gyro_Y)
{
int PWM_out;
PWM_out=Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_Y-0); //gyro_Y-0 ：角速度偏差
return PWM_out;
}
直立环采用的是PD控制器，即out = Kp*Ek+Kd*Ek_D，Ek_D表示偏差的微分
2、速度环
/*********************
速度环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; //低通滤波系数
Encoder_Err = ((encoder_left+encoder_right)-Target); //计算速度偏差
//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;//防止速度过大的影响直立环的正常工作。
Encoder_S += EnC_Err_Lowout;//对速度偏差积分，积分出位移
Encoder_S = Encoder_S > 10000 ? 10000 : (Encoder_S < (-10000) ? (-10000) :Encoder_S);//积分限幅
PWM_out=Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;//速度环控制输出计算
return PWM_out; //输出
}
速度环采用的是PI控制器，即out = Kp*Ek+Ki*Ek_S，Ek_S表示偏差的积分
3、转向环
/*********************
转向环：系数*Z轴角速度
*********************/
int Turn(int gyro_Z)
{
int PWM_out;
PWM_out=(-0.6)*gyro_Z;
// PWM_out=Turn_Kp*gyro_Z;
return PWM_out;
}
转向环比较简单，输出就是一个系数乘以Z轴角速度就可以了
六、控制函数
代码如下
void EXTI9_5_IRQHandler(void) //表示10ms中断时间到
{
int PWM_out;
if(EXTI_GetITStatus(EXTI_Line5)!=0){//一级判定,看陀螺仪有没有中断
if(PBin(5)==0){//二级判定 看看有没有数据 低电平
EXTI_ClearITPendingBit(EXTI_Line5);//清除中断标志位
//采集编码器数据
Encoder_Left=-Read_Speed(3);
Encoder_Right=Read_Speed(4);
//采集陀螺仪角度信息
mpu_dmp_get_data(&Pitch,&Roll,&Yaw); //角度
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //陀螺仪
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //加速度
//将数据压入闭环控制中，计算出控制输出量。
Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); //速度环
Vertical_out=Vertical(Velocity_out+Med_Angle,Pitch,gyroy); //直立环
Turn_out=Turn(gyroz); //转向环
PWM_out=Vertical_out;//最终输出
//把控制输出量加载到电机上，完成最终的的控制。
MOTO1=PWM_out-Turn_out;
MOTO2=PWM_out+Turn_out;
Limit(&MOTO1,&MOTO2); //限幅
Load(MOTO1,MOTO2); //加载到电机上
}
}
}
这里就应该和前面PB5中断函数联系起来了，这里我利用的是陀螺仪采集完数据会产生中断，(这里设置陀螺仪采集数据的周期为10ms)，然后就触发单片机中断，进入中断完成相应的控制。
(当然了，这里也可以不用陀螺仪中断，直接采用定时器定时10ms,然后进入定时器中断函数完成相应的控制，所以这里是非常灵活的。)
其实这个控制函数也是比较简单的，进入中断后先清除中断标志位，再读取陀螺仪和编码器的数据，将数据再压入PID控制器进行计算，计算完成后将相应的值加载到电机上就可以了，进入中断调整一次小车的姿态，一直不断的调整，这样就大概完成了平衡小车的制作。
七、扩展篇
如果有人想在小车上添加一些功能该怎么办呢？比如实现超声波避障，跟随，或者是实现蓝牙控制小车
当然了这也是可以实现的了，在速度环函数的入口参数中有一个Target_Speed变量就是期望速度，可以将这个变量单独拿出来作为二次开发接口，然后改变这个变量的值来进行小车的移动。
比如下面就是实现蓝牙控制的一部分代码
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //加速度
/*前后*/
if((Fore==0)&&(Back==0))Target_Speed=0;//速度清零，稳在原地
if(Fore==1)Target_Speed-=2;//需要前进
if(Back==1)Target_Speed+=2;//
Target_Speed=Target_Speed>SPEED_Y?SPEED_Y:(Target_Speed<-SPEED_Y?(-SPEED_Y):Target_Speed);//限幅
/*左右*/
if((Left==0)&&(Right==0))Turn_Speed=0;
if(Left==1)Turn_Speed+=30; //左转
if(Right==1)Turn_Speed-=30; //右转
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.6;//没有左右转向指令，则转向约束
else if((Left==1)||(Right==1))Turn_Kd=0;//收到左右转向指令，去掉转向约束
Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); //速度环
如果想要添加超声波也是类似的方法，都是改变期望速度Target_Speed
八、效果展示
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
