点个免费的赞赞行不,你的鼓励就是俺的动力~~
前言
这是我第一次写博客,还不太熟练,可能也没有写的很规范,大家见谅啦!
本文只是对STM32平衡小车制作中的代码部分,不涉及原理,加入了我在学习代码过程遇到的困惑以及解答。后续将会写写平衡小车其他部分的博客。本人目前也在学习,如果发现疑点或错误,希望大家能帮我指出来,阿里嘎多啦!
一、基本外设配置
基本外设配置主要包括了与mpu6050有关的外部中断部分;与轮子启动有关的电机驱动部分;与最终调控电机输出数值、电机运动方向有关的电机部分;与编码器数值读取与轮子速度读取有关的部分;用于mpu6050数据可视化的OLED部分。
需要注意的其中的函数又可以分为两大类,配置函数——布置环境;自定义函数——数据自取自用
-
编码器部分
1.配置函数
核心
- 初始化定时器和通道对应IO的时钟;
- 初始化IO口,模式为输入。调用函数:GPIO_Init();
- 初始化定时器ARR,PSC。调用函数:TIM_TimeBaseInit();
- 初始化输入捕获通道。调用函数:TIM_ICInit();
- 如果要开启捕获中断。调用函数:TIM_ITConfig();NVIC_Init();
- 使能定时器。调用函数:TIM_Cmd();
- 编写中断服务函数。调用函数:TIMx_IRQHandler()。
使用定时器中的输入捕获功能,读取连接编码器AB相的引脚(默认定时器的通道一与通道二)检测到的边沿,传递到到编码器接口,并直接利用CNT计数器进行计数。
时钟
打开GPIO引脚、定时器挂载的总线时钟。
GPIO配置
由于是作为输入通道的引脚,接收边沿信息,因此不用开启复用功能(时钟处没有配置)。也因此配置为浮空输入(浮空一般用来做ADC输入用,这样可以减少上下拉电阻对结果)。
定时器配置
时钟不分割。这个colck_division时钟分割系数并不是对定时器的时钟频率进行分割。我们都知道输入捕获模式下有一个数字滤波器,这个数字滤波器可以通过配置寄存器改变他的采样频率,从而将一些频率滤除。
这里滤波器的作用是什么意思呢?数字滤波器由一个事件计数器组成,它记录到N个事件后会产生一个输出的跳变。也就是说连续N次采样,如果都是高电平,则说明这是一个有效的触发,就会进入输入捕捉中断(如果设置了的话)。这样就可以滤除那些高电平脉宽低于8个采样周期的脉冲信号,从而达到滤波的作用。
设置输入捕捉极性为上升沿。这里是设置捕捉事件是发生在上升沿还是下降沿。
自动重装载值为最大值65535。作为边沿跳变计数器,此时以编码器模式进行计数。
注意:有的输入捕获输出计数值为定时器的值,输入捕获作为中断触发条件;有的输入捕获输出计数值为边沿跳变值,证明计数值CNT寄存器模式设置的决定作用
预分频系数为0,不用到计时器的功能。
初始化输入捕获
设置滤波器为10。 在这里,将其设置为10,表示输入信号经过滤波器后,只有高于10个计数单位的脉冲才会被捕获。
使用STM32自带的编码器函数,用于设置输入捕获通道,用于补充TIM_ICInit()。
代码
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);
}
//中断服务函数
void TIM2_IRQHandler(void)
{
if(TIM_GetITStatus(TIM2,TIM_IT_Update)!=0)
{
TIM_ClearITPendingBit(TIM2,TIM_IT_Update);
}
}
void TIM4_IRQHandler(void)
{
if(TIM_GetITStatus(TIM4,TIM_IT_Update)!=0)
{
TIM_ClearITPendingBit(TIM4,TIM_IT_Update);
}
}
2.自定义函数
TIM_GetCounter(TIMx) 读取TIMx寄存器CNT中的计数值
TIM_GetCounter(TIMx) 设置TIMx寄存器CNT中的计数值
TIM_GetCapture2(TIMx)读取TIMx捕获通道2发生捕获时保存在捕获比较寄存器CCR2中的计数器CNT的值
代码
/**********************
编码器
速度读取函数
入口参数:定时器
**********************/
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;
}
-
电机驱动
配置函数
时钟
复用、GPIO、定时器
GPIO初始化
由于引脚作为输出通道,使用复用推免
定时器初始化
时钟不分割,向上计数
输出通道配置
使用PWM1,计数值小于比较值部分起作用
起作用部分为有效电平,且输出极性为高电平
注意:
1.高级定时器专属--MOE主输出使能
2.时钟与输出通道的预装载值都需使能
代码
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);//开定时器。
}
-
外部中断
配置函数
时钟
复用、GPIO
GPIO初始化
引脚作为中断线,配置上拉输入,由于mpu6050当数据寄存器写满时发生下降沿触发中断函数传输数据
中断线配置
PB5即选择Line5中断线
中断模式
配置下降沿触发,由于mpu6050当数据寄存器写满时发生下降沿触发中断函数传输数据
注意
1.使用GPIO与外部中断的映射函数
2.由于mpu6050设置的是10ms的控制周期,因此需要在mpu6050.c文件与inv_mpu.c中进行修改
代码
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);
}
-
电机部分
1.配置函数
时钟为GPIO,将GPIO引脚一律作为推挽输出PWM
2.自定义函数
限幅函数
利用指针,对于最终电机输出比较值进行限幅,使其在规定范围之内
绝对值函数
由于PWM比较值设置必须大于0,考虑到电机反转时输出的值为负的情况
赋值函数
分别确定控制电机的速度的PWM输出比较值与控制电机正反转的引脚01输出
代码
#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--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);
}
/*限幅函数*/
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;
}
/*绝对值函数*/
int GFP_abs(int p)
{
int q;
q=p>0?p:(-p);
return q;
}
/*赋值函数*/
/*入口参数:PID运算完成后的最终PWM值*/
void Load(int moto1,int moto2)//moto1=-200:反转200个脉冲
{
//1.研究正负号,对应正反转
if(moto1>0) Ain1=1,Ain2=0;//正转
else Ain1=0,Ain2=1;//反转
//2.研究PWM值
TIM_SetCompare1(TIM1,GFP_abs(moto1));
if(moto2>0) Bin1=1,Bin2=0;
else Bin1=0,Bin2=1;
TIM_SetCompare4(TIM1,GFP_abs(moto2));
}
-
OLED
使用IIC通讯的方式
二、控制函数
直立环、速度环、转向环的自定义函数,用于采集mpu6050数据、分析处理数据、输出最终PWM值的外部中断函数
1.直立环
参数:期望角度、真实角度、Y方向角速度(作为微分使用,对时间求导)
代码
/*********************
直立环PD控制器:Kp*Ek+Kd*Ek_D
入口:期望角度、真实角度、真实角速度
出口:直立环输出
*********************/
int Vertical(float Med,float Angle,float gyro_Y)
{
int PWM_out;
PWM_out=Vertical_Kp*Angle+Vertical_Kd*(gyro_Y-0);//【1】
return PWM_out;
}
2.速度环
参数:左右电机编码器记录边沿数
注意:
1.对速度偏差进行低通滤波,引入常数a值。对积分使用的偏差值按比例从前一次偏差值与本次偏差值获取。一方面,使得波形更加平滑,滤除高频干扰,防止速度突变;另一方面,体现以直立环为主,速度环为辅助型的策略。
2.积分限幅
代码
/*********************
速度环PI:Kp*Ek+Ki*Ek_S
*********************/
int Velocity(int encoder_left,int encoder_right)
{
static int PWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last;//【2】
float a=0.7;//【3】
//1.计算速度偏差
Encoder_Err=(encoder_left+encoder_right)-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】
//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;//【5】
return PWM_out;
}
3.转向环
参数:Z轴角速度(使用P比例系数,抵消左右电机产生的速度差)
代码
/*********************
转向环:系数*Z轴角速度
*********************/
int Turn(int gyro_Z)
{
int PWM_out;
PWM_out=Turn_Kp*gyro_Z;
return PWM_out;
}
4.外部中断函数
1.中断判定:判断中断标志位的自带函数;判断中断引脚PB5是否拉低
2.清除中断标志位
3.采集信息(mpu6050——自带、电机转速——自定义)
mpu_dmp_get_data(&Pitch,&Roll,&Yaw); //角度
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //陀螺仪
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //加速度
4.数据处理,进行闭环控制中获取新数据(直立环、速度环、转向环的对应PWM输出量,而后进行归一化计算,得出最终的PWM输出值)
5.将控制量加载到电机
注意
此处并不是严格按照计算公式进行直立环的计算,而是在经串级控制系统计算后,近似的结果,属于视频中的推导公式版本。
代码
void EXTI9_5_IRQHandler(void)
{
if(EXTI_GetITStatus(EXTI_Line5)!=0)//一级判定
{
int PWM_out;
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); //加速度
//2.将数据压入闭环控制中,计算出控制输出量。
Vertical_out=Vertical(Med_Angle,Pitch,gyroy); //直立环
Velocity_out=Velocity(Encoder_Left,Encoder_Right); //速度环
Turn_out=Turn(gyroz); //转向环
PWM_out=Vertical_out-Vertical_Kp*Velocity_out;//最终输出
//3.把控制输出量加载到电机上,完成最终的的控制。
MOTO1=PWM_out-Turn_out;//左电机
MOTO2=PWM_out+Turn_out;//右电机
Limit(&MOTO1,&MOTO2);//PWM限幅
Load(MOTO1,MOTO2);//加载到电机上。
}
}
}
三、主函数
变量:
- 控制函数:机械中值、PID调节参数、直立环&速度环&转向环 的输出变量为宏定义
- 自定义函数:偏差值......
- 主函数:
float Pitch,Roll,Yaw; //角度
short gyrox,gyroy,gyroz; //陀螺仪--角速度
short aacx,aacy,aacz; //加速度
int Encoder_Left,Encoder_Right; //编码器数据(速度)
int PWM_MAX=7200,PWM_MIN=-7200; //PWM限幅变量
int MOTO1,MOTO2; //电机装载变量
初始化:
delay_init();
NVIC_Config();
uart1_init(115200);
OLED_Init();
OLED_Clear();
MPU_Init();
mpu_dmp_init();
MPU6050_EXTI_Init();
Encoder_TIM2_Init();
Encoder_TIM4_Init();
Motor_Init();
PWM_Init_TIM1(0,7199);
while循环
通过将mpu6050角度值可视化,确定机械中值常量,进行平衡调试。
while(1)
{
OLED_Float(0,0,Pitch,3);
}
四、参考
十分感谢b站大佬天下行走的视频,手把手教会平衡车制作,推荐大家去看原视频http://【平衡小车在线编程课程视频(1~3节合集版本)(2月12日更新)】https://www.bilibili.com/video/BV1j7411z7uX?vd_source=20c38b34f910bca533d1561cedad57ff