目录
一、硬件设计
1、硬件选购
这里建议大家买MPU6050时一定要看好质量，有些买来用一两次就坏了。
2、硬件使用方法及接线图
2.1、编码器
主要由编码器的A、B相接入到STM32定时器I\O口，编码器接口会自动控制定时器时基单元中的CNT计数器进行自增或自减，再通过OLED屏从而获取编码器的位置、旋转速度和方向
具体接线方式
| 红线 | TB6612：AO2、BO1 |
| 白线 | TB6612：AO1、BO2 |
| 黄线（A相） | MCU：PB6 |
| 绿线（B相） | MCU：PB7 |
| 黑线 | GND |
| 蓝线 | 电源模块5v |
2.2、MPU6050
MPU6050是一个6轴姿态传感器 ，内有加速度和陀螺仪两种传感器可以分别测量X、Y、Z的加速度、角速度。综合多种数据的融合（互补滤波或卡尔曼滤波），可进一步获得准确的欧拉角（姿态角） ，内部有集成的DMP姿态解算主要目的是车身位置，从而保持平衡
主要代码如下
#include "mpu6050.h"
#include "sys.h"
#include "delay.h"
#include "usart.h"
//初始化MPU6050
//返回值:0,成功
//其他,错误代码
u8 MPU_Init(void)
{
u8 res;
MPU_IIC_Init();//初始化IIC总线
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //复位MPU6050
delay_ms(100);
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //唤醒MPU6050
MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps
MPU_Set_Accel_Fsr(0); //加速度传感器,±2g
MPU_Set_Rate(200); //设置采样率50Hz
MPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断
MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C主模式关闭
MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFO
MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //INT引脚低电平有效
res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
if(res==MPU_ADDR)//器件ID正确
{
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); //设置CLKSEL,PLL X轴为参考
MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); //加速度与陀螺仪都工作
MPU_Set_Rate(100); //设置采样率为50Hz
}else return 1;
return 0;
}
//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
// 其他,设置失败
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{
return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围
}
//设置MPU6050加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
// 其他,设置失败
u8 MPU_Set_Accel_Fsr(u8 fsr)
{
return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围
}
//设置MPU6050的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
// 其他,设置失败
u8 MPU_Set_LPF(u16 lpf)
{
u8 data=0;
if(lpf>=188)data=1;
else if(lpf>=98)data=2;
else if(lpf>=42)data=3;
else if(lpf>=20)data=4;
else if(lpf>=10)data=5;
else data=6;
return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器
}
//设置MPU6050的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
// 其他,设置失败
u8 MPU_Set_Rate(u16 rate)
{
u8 data;
if(rate>1000)rate=1000;
if(rate<4)rate=4;
data=1000/rate-1;
data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data); //设置数字低通滤波器
return MPU_Set_LPF(rate/2); //自动设置LPF为采样率的一半
}
//得到温度值
//返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void)
{
u8 buf[2];
short raw;
float temp;
MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf);
raw=((u16)buf[0]<<8)|buf[1];
temp=36.53+((double)raw)/340;
return temp*100;;
}
//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
// 其他,错误代码
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
u8 buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
if(res==0)
{
*gx=((u16)buf[0]<<8)|buf[1];
*gy=((u16)buf[2]<<8)|buf[3];
*gz=((u16)buf[4]<<8)|buf[5];
}
return res;;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
// 其他,错误代码
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
u8 buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
if(res==0)
{
*ax=((u16)buf[0]<<8)|buf[1];
*ay=((u16)buf[2]<<8)|buf[3];
*az=((u16)buf[4]<<8)|buf[5];
}
return res;;
}
//IIC连续写
//addr:器件地址
//reg:寄存器地址
//len:写入长度
//buf:数据区
//返回值:0,正常
// 其他,错误代码
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
u8 i;
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令
if(MPU_IIC_Wait_Ack()) //等待应答
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg); //写寄存器地址
MPU_IIC_Wait_Ack(); //等待应答
for(i=0;i<len;i++)
{
MPU_IIC_Send_Byte(buf[i]); //发送数据
if(MPU_IIC_Wait_Ack()) //等待ACK
{
MPU_IIC_Stop();
return 1;
}
}
MPU_IIC_Stop();
return 0;
}
//IIC连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取的长度
//buf:读取到的数据存储区
//返回值:0,正常
// 其他,错误代码
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令
if(MPU_IIC_Wait_Ack()) //等待应答
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg); //写寄存器地址
MPU_IIC_Wait_Ack(); //等待应答
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|1);//发送器件地址+读命令
MPU_IIC_Wait_Ack(); //等待应答
while(len)
{
if(len==1)*buf=MPU_IIC_Read_Byte(0);//读数据,发送nACK
else *buf=MPU_IIC_Read_Byte(1); //读数据,发送ACK
len--;
buf++;
}
MPU_IIC_Stop(); //产生一个停止条件
return 0;
}
//IIC写一个字节
//reg:寄存器地址
//data:数据
//返回值:0,正常
// 其他,错误代码
u8 MPU_Write_Byte(u8 reg,u8 data)
{
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令
if(MPU_IIC_Wait_Ack()) //等待应答
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg); //写寄存器地址
MPU_IIC_Wait_Ack(); //等待应答
MPU_IIC_Send_Byte(data);//发送数据
if(MPU_IIC_Wait_Ack()) //等待ACK
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Stop();
return 0;
}
//IIC读一个字节
//reg:寄存器地址
//返回值:读到的数据
u8 MPU_Read_Byte(u8 reg)
{
u8 res;
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令
MPU_IIC_Wait_Ack(); //等待应答
MPU_IIC_Send_Byte(reg); //写寄存器地址
MPU_IIC_Wait_Ack(); //等待应答
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|1);//发送器件地址+读命令
MPU_IIC_Wait_Ack(); //等待应答
res=MPU_IIC_Read_Byte(0);//读取数据,发送nACK
MPU_IIC_Stop(); //产生一个停止条件
return res;
}
| VCC | 3.3V |
| GND | 接地（GND） |
| SCL、SDA | I2C通信（MCU：PB4、PB3） |
| XDA、XCL | 主机I2C通信也可连接扩展设备 |
| AD0 | 从机地址最低位 |
| INT | 中断信号输出（MCU：PB5） |
2.3TB6612电机驱动
主要使用PWM脉冲来驱动电机，启动定时器1开启两个通道来分别控制2个电机，以下是代码配置
PWM初始化
#include "pwm.h"
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】*///TIM1
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);//开定时器。
}
电机初始化
#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));
}
| PWMA | MCU：PA11 |
| AIN2 | MCU：PB21 |
| AIN1 | MCU：PB13 |
| STBY | 5V |
| BIN1 | MCU：PB14 |
| BIN2 | MCU：PB15 |
| PWMB | MCU：PA8 |
| GND | 接地（GND） |
| VM | 5V |
| VCC | VCC |
| GND | 接地（GND） |
| AO1 | 编码器电源-（白线）（左电机） |
| AO2 | 编码器电源+（红线） |
| BO2 | 编码器电源+（红线）（右电机） |
| BO1 | 编码器电源-（白线） |
| GND | 接地（GND） |
二、PID算法
本次的重中之重，PID相信大家都不陌生，多少听说过，一些基础性的东西我就不赘述。这里就简单的说一下关键参数
公式：
串级控制系统
1、PID调参
1.1、直立环
Kp极性：任意给定值后拿起小车前倾，车轮前转说明极性正确，如果极性错误前面加个负号即可
Kp大小：当给定值不断增大，当小车有低频振荡时说明Kp值为最大值，可引入Kd
Kd极性: Kp值设为0，任意给定Kd值后拿起小车绕电机轴旋转，车轮同相转动，说明极性正确
Kd大小： 恢复Kp值，Kd值不断增大，当小车有高频振荡是说明Kd值为最大值，可引入速度环
注意：直立环调完后两个系数乘0.6再调速度环
1.2、速度环
Kp&Ki极性：手动转动一个车轮，另一个车轮会同向加速，直至最大说明极性正确
Kp&Ki大小：不断增加Kp&Ki值，直至小车保持平衡的同时，速度接近于零。且回位效果好
#include "control.h"
float Med_Angle=0;//机械中值--能使得小车真正平衡住的角度。
float
Vertical_Kp=0, //直立环KP、KD
Vertical_Kd=0;
float
Velocity_Kp=0, //速度环KP、KI
Velocity_Ki=0;
float
Turn_Kp=0;
int Vertical_out,Velocity_out,Turn_out;//直立环&速度环&转向环 的输出变量
int Vertical(float Med,float Angle,float gyro_Y);//函数声明
int Velocity(int encoder_left,int encoder_right);
int Turn(int gyro_Z);
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);//加载到电机上。
}
}
}
/*********************
直立环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;
}
/*********************
速度环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;
}
/*********************
转向环：系数*Z轴角速度
*********************/
int Turn(int gyro_Z)
{
int PWM_out;
PWM_out=Turn_Kp*gyro_Z;
return PWM_out;
}
三、最终效果及开源资料
基于STM32平衡小车（PID+编码器+MPU6050+TB6612）_哔哩哔哩_bilibili
链接：https://pan.baidu.com/s/1Sc7Dvk_EKXP73qSf7FSY1g 提取码：1234
以上就是本次的全部内容，欢迎大家评论，收藏和转发，如有问题请私信或评论
602
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
