我要评论一、引言
关于stm32两轮平衡车的设计，我想在读者阅读本文之前应该已经有所了解，所以本文的重点是代码的分享和分析。至于具体的原理，我觉得读者不必阅读长篇大论的文章，只需按照本文分享的代码自己亲手制作一辆平衡车，其原理并不言而喻了。源完整代码工程在文章末尾百度网盘链接，请需要的读者自行下载即可。
另外，由于平衡车的精髓在于pid算法的运用，有需要了解pid算法的读者可以参考以下两篇文章：
pid算法详解（代码详解篇），位置式pid、增量式pid（通用）_pid 代码-csdn博客
pid算法详解（精华知识汇总）_小小_扫地僧的博客-csdn博客
二、所需材料
1、stm32f03c8t6
2、mpu6050
3、蓝牙模块
4、编码电机
5、tb6612
6、电源+稳压模块
7、oled显示模块
三、接线强调
1、tb6612接线
2、蓝牙模块与单片机之间
3、mpu6050
使用iic通信，所以对照代码接sda、scl、gnd、vcc、in（中断触发线）
四、功能介绍
五、关键算法
pid算法对编码电机的控制
1.位置闭环控制
1.1理论分析
1.2控制原理图
1.3c语言实现
int position_pid (int encoder, int target)
{
static float bias, pwm,integral_bias,last_bias;
bias=encoder-target;//计算偏差
integral_bias+=bias; //求出偏差的积分
pwm=position_kp*bias+position_ki*integral_bias+position_kd*(bias-last_bias);last_bias=bias; //保存上一次偏差
return pwm; //输出
}
2、速度闭环控制
2.1理论分析
2.2 控制原理图
2.3 c语言实现
增量式pi控制器具体通过c语言实现的代码如下:
int incremental_pi (int encoder,int target)
{
static float bias, pwm, last_bias;
bias=encoder-target;//计算偏差
pwm+=velocity_kp*(bias-last_bias)+velocity_ki*bias;//增量式pi控制器
last_bias=bias;//保存上一次偏差
return pwm;//增量输出
}六、关键代码分析
1、编码电机pid算法控制
#include "control.h"
#include "usart2.h"
/**************************************************************************
函数功能：所有的控制代码都在这里面
5ms定时中断由mpu6050的int引脚触发
严格保证采样和数据处理的时间同步
在mpu6050的采样频率设置中，设置成100hz，即可保证6050的数据是10ms更新一次。
读者可在imv_mpu.h文件第26行的宏定义进行修改(#define default_mpu_hz (100))
**************************************************************************/
#define speed_y 100 //俯仰(前后)最大设定速度
#define speed_z 80//偏航(左右)最大设定速度
int balance_pwm,velocity_pwm,turn_pwm,turn_kp;
float mechanical_angle=8;
float target_speed=0; //期望速度（俯仰）。用于控制小车前进后退及其速度。
float turn_speed=0; //期望速度（偏航）
//针对不同车型参数，在sys.h内设置define的电机类型
float balance_up_kp=blc_kp; // 小车直立环pd参数
float balance_up_kd=blc_kd;
float velocity_kp=spd_kp; // 小车速度环pi参数
float velocity_ki=spd_ki;
float turn_kd=turn_kd;//转向环kp、kd
float turn_kp=turn_kp;
void exti9_5_irqhandler(void)
{
static u8 voltage_counter=0;
if(pbin(5)==0)
{
exti->pr=1<<5; //清除line5上的中断标志位
mpu_dmp_get_data(&pitch,&roll,&yaw); //得到欧拉角（姿态角）的数据
mpu_get_gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺仪数据
encoder_left=read_encoder(2); //读取编码器的值，保证输出极性一致
encoder_right=-read_encoder(3); //读取编码器的值
led_flash(100);
voltage_counter++;
if(voltage_counter==20) //100ms读取一次电压
{
voltage_counter=0;
voltage=get_battery_volt(); //读取电池电压
}
if(key_press(100)) //长按按键切换模式并触发模式切换初始化
{
if(++ctrl_mode>=101)
ctrl_mode=97;
mode_change=1;
}
get_rc();
target_speed=target_speed>speed_y?speed_y:(target_speed<-speed_y?(-speed_y):target_speed);//限幅
turn_speed=turn_speed>speed_z?speed_z:(turn_speed<-speed_z?(-speed_z):turn_speed);//限幅( (20*100) * 100)
balance_pwm =balance_up(pitch,mechanical_angle,gyroy); //===直立环pid控制
velocity_pwm=velocity(encoder_left,encoder_right,target_speed); //===速度环pid控制
turn_pwm =turn_up(gyroz,turn_speed); //===转向环pid控制
moto1=balance_pwm-velocity_pwm+turn_pwm; //===计算左轮电机最终pwm
moto2=balance_pwm-velocity_pwm-turn_pwm; //===计算右轮电机最终pwm
xianfu_pwm(); //===pwm限幅
turn_off(pitch,12); //===检查角度以及电压是否正常
set_pwm(moto1,moto2); //===赋值给pwm寄存器
}
}
/**************************************************************************
函数功能：直立pd控制
入口参数：角度、机械平衡角度（机械中值）、角速度
返回 值：直立控制pwm
**************************************************************************/
int balance_up(float angle,float mechanical_balance,float gyro)
{
float bias;
int balance;
bias=angle-mechanical_balance; //===求出平衡的角度中值和机械相关
balance=balance_up_kp*bias+balance_up_kd*gyro; //===计算平衡控制的电机pwm pd控制 kp是p系数 kd是d系数
return balance;
}
/**************************************************************************
函数功能：速度pi控制
入口参数：电机编码器的值
返回 值：速度控制pwm
**************************************************************************/
int velocity(int encoder_left,int encoder_right,int target_speed)
{
static float velocity,encoder_least,encoder;
static float encoder_integral;
//=============速度pi控制器=======================//
encoder_least =(encoder_left+encoder_right);//-target; //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度
encoder *= 0.8; //===一阶低通滤波器
encoder += encoder_least*0.2; //===一阶低通滤波器
encoder_integral +=encoder; //===积分出位移 积分时间：10ms
encoder_integral=encoder_integral - target_speed; //===接收遥控器数据，控制前进后退
if(encoder_integral>10000) encoder_integral=10000; //===积分限幅
if(encoder_integral<-10000) encoder_integral=-10000; //===积分限幅
velocity=encoder*velocity_kp+encoder_integral*velocity_ki; //===速度控制
if(pitch<-40||pitch>40) encoder_integral=0; //===电机关闭后清除积分
return velocity;
}
/**************************************************************************
函数功能：转向pd控制
入口参数：电机编码器的值、z轴角速度
返回 值：转向控制pwm
**************************************************************************/
int turn_up(int gyro_z, int rc)
{
int pwm_out;
/*转向约束*/
if(rc==0)
turn_kd=turn_kd; //若无左右转向指令，则开启转向约束
else
turn_kd=0; //若左右转向指令接收到，则去掉转向约束
pwm_out=turn_kd*gyro_z + turn_kp*rc;
return pwm_out;
}
void tracking()
{
tksensor=0;
tksensor+=(c1<<3);
tksensor+=(c2<<2);
tksensor+=(c3<<1);
tksensor+=c4;
}
void get_rc()
{
static u8 sr04_counter =0;
static float rate_vel = 1;
float rate_turn = 1.6;
float ly,rx; //ps2手柄控制变量
int yuzhi=2; //ps2控制防抖阈值
switch(ctrl_mode)
{
case 97:
sr04_counter++;
if(sr04_counter>=20) //100ms读取一次超声波的数据
{
sr04_counter=0;
sr04_startmeasure(); //读取超声波的值
}
if(sr04_distance<=30)
{
target_speed=0,turn_speed=40;
}
else
{
target_speed=30,turn_speed=0;
}
break;
case 98://蓝牙模式
if((fore==0)&&(back==0))
target_speed=0;//未接受到前进后退指令-->速度清零，稳在原地
if(fore==1)
target_speed--;//前进1标志位拉高-->需要前进
if(back==1)
target_speed++;//
/*左右*/
if((left==0)&&(right==0))
turn_speed=0;
if(left==1)
turn_speed-=30; //左转
if(right==1)
turn_speed+=30; //右转
break;
case 99://循迹模式
tracking();
switch(tksensor)
{
case 15:
target_speed=0;
turn_speed=0;
break;
case 9:
target_speed--;
turn_speed=0;
break;
case 2://向右转
target_speed--;
turn_speed=15;
break;
case 4://向左转
target_speed--;
turn_speed=-15;
break;
case 8:
target_speed=-10;
turn_speed=-80;
break;
case 1:
target_speed=-10;
turn_speed=80;
break;
}
break;
case 100://ps2手柄遥控
if(ps2_plugin)
{
ly=ps2_ly-128; //获取偏差
rx=ps2_rx-128; //获取偏差
if(ly>-yuzhi&&ly<yuzhi)
ly=0; //设置小角度的死区
if(rx>-yuzhi&&rx<yuzhi)
rx=0; //设置小角度的死区
if(target_speed>-ly/rate_vel)
target_speed--;
else if(target_speed<-ly/rate_vel)
target_speed++;
turn_speed=rx/rate_turn;
}
else
{
target_speed=0,turn_speed=0;
}
break;
}
}
2、编码电机编码值采集
#include "encoder.h"
/**************************************************************************
函数功能：把tim2初始化为编码器接口模式
入口参数：无
返回 值：无
**************************************************************************/
void encoder_init_tim2(void)
{
tim_timebaseinittypedef tim_timebasestructure;
tim_icinittypedef tim_icinitstructure;
gpio_inittypedef gpio_initstructure;
rcc_apb1periphclockcmd(rcc_apb1periph_tim2, enable);//使能定时器4的时钟
rcc_apb2periphclockcmd(rcc_apb2periph_gpioa, enable);//使能pb端口时钟
gpio_initstructure.gpio_pin = gpio_pin_0|gpio_pin_1; //端口配置
gpio_initstructure.gpio_mode = gpio_mode_in_floating; //浮空输入
gpio_init(gpioa, &gpio_initstructure); //根据设定参数初始化gpiob
tim_timebasestructinit(&tim_timebasestructure);
tim_timebasestructure.tim_prescaler = 0x0; // 预分频器
tim_timebasestructure.tim_period = encoder_tim_period; //设定计数器自动重装值
tim_timebasestructure.tim_clockdivision = tim_ckd_div1;//选择时钟分频：不分频
tim_timebasestructure.tim_countermode = tim_countermode_up;tim向上计数
tim_timebaseinit(tim2, &tim_timebasestructure);
tim_encoderinterfaceconfig(tim2, tim_encodermode_ti12, tim_icpolarity_rising, tim_icpolarity_rising);//使用编码器模式3
tim_icstructinit(&tim_icinitstructure);
tim_icinitstructure.tim_icfilter = 10;
tim_icinit(tim2, &tim_icinitstructure);
tim_clearflag(tim2, tim_flag_update);//清除tim的更新标志位
tim_itconfig(tim2, tim_it_update, enable);
//reset counter
tim_setcounter(tim2,0);
tim_cmd(tim2, enable);
}
/**************************************************************************
函数功能：把tim3初始化为编码器接口模式
入口参数：无
返回 值：无
**************************************************************************/
void encoder_init_tim3(void)
{
tim_timebaseinittypedef tim_timebasestructure;
tim_icinittypedef tim_icinitstructure;
gpio_inittypedef gpio_initstructure;
rcc_apb1periphclockcmd(rcc_apb1periph_tim3, enable);//使能定时器4的时钟
rcc_apb2periphclockcmd(rcc_apb2periph_gpioa, enable);//使能pb端口时钟
gpio_initstructure.gpio_pin = gpio_pin_6|gpio_pin_7; //端口配置
gpio_initstructure.gpio_mode = gpio_mode_in_floating; //浮空输入
gpio_init(gpioa, &gpio_initstructure); //根据设定参数初始化gpiob
tim_timebasestructinit(&tim_timebasestructure);
tim_timebasestructure.tim_prescaler = 0x0; // 预分频器
tim_timebasestructure.tim_period = encoder_tim_period; //设定计数器自动重装值
tim_timebasestructure.tim_clockdivision = tim_ckd_div1;//选择时钟分频：不分频
tim_timebasestructure.tim_countermode = tim_countermode_up;tim向上计数
tim_timebaseinit(tim3, &tim_timebasestructure);
tim_encoderinterfaceconfig(tim3, tim_encodermode_ti12,tim_icpolarity_rising, tim_icpolarity_rising);//使用编码器模式3
tim_icstructinit(&tim_icinitstructure);
tim_icinitstructure.tim_icfilter = 10;
tim_icinit(tim3, &tim_icinitstructure);
tim_clearflag(tim3, tim_flag_update);//清除tim的更新标志位
tim_itconfig(tim3, tim_it_update, enable);
//reset counter
tim_setcounter(tim3,0);
tim_cmd(tim3, enable);
}
/**************************************************************************
函数功能：单位时间读取编码器计数
入口参数：定时器
返回 值：速度值
**************************************************************************/
int read_encoder(u8 timx)
{
int encoder_tim;
switch(timx)
{
case 2:
encoder_tim= (short)tim2 -> cnt;
tim2 -> cnt=0;
break;
case 3:
encoder_tim= (short)tim3 -> cnt; tim3 -> cnt=0;
break;
default: encoder_tim=0;
}
return encoder_tim;
}
3、pwm配置
#include "pwm.h"
//pwm输出初始化
//arr：自动重装值
//psc：时钟预分频数
//tim1_pwm_init(7199,0);//pwm频率=72000/(7199+1)=10khz
void tim1_pwm_init(u16 arr,u16 psc)
{
gpio_inittypedef gpio_initstructure;
tim_timebaseinittypedef tim_timebasestructure;
tim_ocinittypedef tim_ocinitstructure;
rcc_apb2periphclockcmd(rcc_apb2periph_tim1, enable);//
rcc_apb2periphclockcmd(rcc_apb2periph_gpioa , enable); //使能gpio外设时钟使能
//设置该引脚为复用输出功能,输出tim1 ch1 ch4的pwm脉冲波形
gpio_initstructure.gpio_pin = gpio_pin_8|gpio_pin_11; //tim_ch1 //tim_ch4
gpio_initstructure.gpio_mode = gpio_mode_af_pp; //复用推挽输出
gpio_initstructure.gpio_speed = gpio_speed_50mhz;
gpio_init(gpioa, &gpio_initstructure);
tim_timebasestructure.tim_period = arr; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值
tim_timebasestructure.tim_prescaler =psc; //设置用来作为timx时钟频率除数的预分频值 不分频
tim_timebasestructure.tim_clockdivision = 0; //设置时钟分割:tdts = tck_tim
tim_timebasestructure.tim_countermode = tim_countermode_up; //tim向上计数模式
tim_timebaseinit(tim1, &tim_timebasestructure); //根据tim_timebaseinitstruct中指定的参数初始化timx的时间基数单位
tim_ocinitstructure.tim_ocmode = tim_ocmode_pwm1; //选择定时器模式:tim脉冲宽度调制模式1
tim_ocinitstructure.tim_outputstate = tim_outputstate_enable; //比较输出使能
tim_ocinitstructure.tim_pulse = 0; //设置待装入捕获比较寄存器的脉冲值
tim_ocinitstructure.tim_pulse = arr >> 1;
tim_ocinitstructure.tim_ocpolarity = tim_ocpolarity_high; //输出极性:tim输出比较极性高
tim_oc1init(tim1, &tim_ocinitstructure); //根据tim_ocinitstruct中指定的参数初始化外设timx
tim_oc4init(tim1, &tim_ocinitstructure); //根据tim_ocinitstruct中指定的参数初始化外设timx
tim_ctrlpwmoutputs(tim1,enable); //moe 主输出使能
tim_oc1preloadconfig(tim1, tim_ocpreload_enable); //ch1预装载使能
tim_oc4preloadconfig(tim1, tim_ocpreload_enable); //ch4预装载使能
tim_arrpreloadconfig(tim1, enable); //使能timx在arr上的预装载寄存器
tim_cmd(tim1, enable); //使能tim1
}
4、蓝牙控制
#include "usart2.h"
/**************************************************************************
函数功能：串口2初始化
入口参数： bound:波特率
返回 值：无
**************************************************************************/
void uart2_init(u32 bound)
{
//gpio端口设置
gpio_inittypedef gpio_initstructure;
usart_inittypedef usart_initstructure;
rcc_apb2periphclockcmd(rcc_apb2periph_gpioa, enable); //使能ugpiob时钟
rcc_apb1periphclockcmd(rcc_apb1periph_usart2, enable); //使能usart2时钟
//usart2_tx
gpio_initstructure.gpio_pin = gpio_pin_2; //pa2
gpio_initstructure.gpio_speed = gpio_speed_50mhz;
gpio_initstructure.gpio_mode = gpio_mode_af_pp; //复用推挽输出
gpio_init(gpioa, &gpio_initstructure);
//usart2_rx
gpio_initstructure.gpio_pin = gpio_pin_3;//pa3
gpio_initstructure.gpio_mode = gpio_mode_in_floating;//浮空输入
gpio_init(gpioa, &gpio_initstructure);
//usart 初始化设置
usart_initstructure.usart_baudrate = bound;//串口波特率
usart_initstructure.usart_wordlength = usart_wordlength_8b;//字长为8位数据格式
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(usart2, &usart_initstructure); //初始化串口2
usart_itconfig(usart2, usart_it_rxne, enable);//开启串口接受中断
usart_cmd(usart2, enable); //使能串口2
}
/**************************************************************************
函数功能：串口2接收中断
入口参数：无
返回 值：无
**************************************************************************/
u8 fore,back,left,right;
void usart2_irqhandler(void)
{
int uart_receive;
if(usart_getitstatus(usart2,usart_it_rxne)!=reset)//接收中断标志位拉高
{
uart_receive=usart_receivedata(usart2);//保存接收的数据
bluetoothcmd(uart_receive);
}
}
void bluetoothcmd(int uart_receive)
{
switch(uart_receive)
{
case 90://停止
fore=0,back=0,left=0,right=0;
break;
case 65://前进
fore=1,back=0,left=0,right=0;
break;
case 72://左前
fore=1,back=0,left=1,right=0;
break;
case 66://右前
fore=1,back=0,left=0,right=1;
break;
case 71://左转
fore=0,back=0,left=1,right=0;
break;
case 67://右转
fore=0,back=0,left=0,right=1;
break;
case 69://后退
fore=0,back=1,left=0,right=0;
break;
case 70://左后,向右旋
fore=0,back=1,left=0,right=1;
break;
case 68://右后，向左旋
fore=0,back=1,left=1,right=0;
break;
default://停止
fore=0,back=0,left=0,right=0;
break;
}
}
void uart2sendbyte(char byte) //串口发送一个字节
{
usart_senddata(usart2, byte); //通过库函数 发送数据
while( usart_getflagstatus(usart2,usart_flag_tc)!= set);
//等待发送完成。 检测 usart_flag_tc 是否置1； //见库函数 p359 介绍
}
void uart2sendbuf(char *buf, u16 len)
{
u16 i;
for(i=0; i<len; i++)uart2sendbyte(*buf++);
}
void uart2sendstr(char *str)
{
u16 i,len;
len = strlen(str);
for(i=0; i<len; i++)uart2sendbyte(*str++);
}
5、中断处理函数
void exti9_5_irqhandler(void)
{
static u8 voltage_counter=0;
if(pbin(5)==0)
{
exti->pr=1<<5; //清除line5上的中断标志位
mpu_dmp_get_data(&pitch,&roll,&yaw); //得到欧拉角（姿态角）的数据
mpu_get_gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺仪数据
encoder_left=read_encoder(2); //读取编码器的值，保证输出极性一致
encoder_right=-read_encoder(3); //读取编码器的值
led_flash(100);
voltage_counter++;
if(voltage_counter==20) //100ms读取一次电压
{
voltage_counter=0;
voltage=get_battery_volt(); //读取电池电压
}
if(key_press(100)) //长按按键切换模式并触发模式切换初始化
{
if(++ctrl_mode>=101)
ctrl_mode=97;
mode_change=1;
}
get_rc();
target_speed=target_speed>speed_y?speed_y:(target_speed<-speed_y?(-speed_y):target_speed);//限幅
turn_speed=turn_speed>speed_z?speed_z:(turn_speed<-speed_z?(-speed_z):turn_speed);//限幅( (20*100) * 100)
balance_pwm =balance_up(pitch,mechanical_angle,gyroy); //===直立环pid控制
velocity_pwm=velocity(encoder_left,encoder_right,target_speed); //===速度环pid控制
turn_pwm =turn_up(gyroz,turn_speed); //===转向环pid控制
moto1=balance_pwm-velocity_pwm+turn_pwm; //===计算左轮电机最终pwm
moto2=balance_pwm-velocity_pwm-turn_pwm; //===计算右轮电机最终pwm
xianfu_pwm(); //===pwm限幅
turn_off(pitch,12); //===检查角度以及电压是否正常
set_pwm(moto1,moto2); //===赋值给pwm寄存器
}
}七、pcb板设计
八、代码开源
1、寄存器版本
链接：https://pan.baidu.com/s/1nlmhsgmf2cu8sz955n27eg?pwd=zxf1
提取码：zxf1
--来自百度网盘超级会员v2的分享
2、hal库版本
链接：https://pan.baidu.com/s/1rw5m7dz-tk4iwjxnp57mbw?pwd=zxf1
提取码：zxf1
--来自百度网盘超级会员v2的分享
发表评论