目录
一、硬件设计
1、硬件选择
主控芯片 | STM32F103C8T6 |
传感器 | MPU6050 |
电机驱动 | TB6612 |
电机 | N20编码器减速电机 |
蓝牙(可选) | HC-05 |
电池 | 18650锂电池*2 |
电源转换 | DC-DC可调降压模块 |
2、硬件框图
3、分模块说明以及相关代码
3.1、电机驱动TB6612
具体原理以及逻辑真值表:
通过接收PWM信号,根据输入信号的逻辑电平确定电机的运动方向,可以使电机正转、反转或停止。
A/BIN1 | 0 | 0 | 1 |
A/BIN2 | 0 | 1 | 0 |
转向 | 停止 | 正转 | 反转 |
具体接线:
VCC | 3.3V电源 |
VM | 7.4V电源 |
GND | 地 |
PWMA、B | PA8、9 |
AIN1、2 | PA4、5 |
BIN1、2 | PA0、1 |
代码部分(含PWM):
#define Left_Motor_turn_Positive GPIO_SetBits(GPIOA,GPIO_Pin_4),GPIO_ResetBits(GPIOA,GPIO_Pin_5) //左电机正转
#define Left_Motor_turn_Negative GPIO_SetBits(GPIOA,GPIO_Pin_5),GPIO_ResetBits(GPIOA,GPIO_Pin_4) //左电机反转
#define Right_Motor_turn_Positive GPIO_SetBits(GPIOA,GPIO_Pin_0),GPIO_ResetBits(GPIOA,GPIO_Pin_1) //右电机正转
#define Right_Motor_turn_Negative GPIO_SetBits(GPIOA,GPIO_Pin_1),GPIO_ResetBits(GPIOA,GPIO_Pin_0) //右电机反转
void PWM_Init(void)
{
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1,ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
GPIO_InitTypeDef GPIO_InitStruct;
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_OCInitTypeDef TIM_OCInitStruct;
//PWM
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_9; //TIM1_CH1/2
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStruct);
//输出IO
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_Out_PP; //推挽输出
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_4|GPIO_Pin_5;
GPIO_Init(GPIOA,&GPIO_InitStruct);
GPIO_ResetBits(GPIOA,GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_4|GPIO_Pin_5); //默认低电平(停止)
TIM_InternalClockConfig(TIM1); //选择时基单元
TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInitStruct.TIM_Period = 7200-1; //ARR重装载值 1khz
TIM_TimeBaseInitStruct.TIM_Prescaler = 10-1; //PSC预分频值
TIM_TimeBaseInitStruct.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct);
TIM_OCStructInit(&TIM_OCInitStruct);
TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1; //PWM1模式
TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High; //有效电平为高电平
TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable; //输出使能
TIM_OCInitStruct.TIM_Pulse = 0; //CCR比较值
TIM_OC1Init(TIM1,&TIM_OCInitStruct); //通道1,2初始化
TIM_OC2Init(TIM1,&TIM_OCInitStruct);
TIM_CtrlPWMOutputs(TIM1,ENABLE); //MOE主输出使能
TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Enable); //CH1预装载使能
TIM_OC2PreloadConfig(TIM1, TIM_OCPreload_Enable); //CH2预装载使能
TIM_ARRPreloadConfig(TIM1, ENABLE); //使能TIMx在ARR上的预装载寄存器
TIM_Cmd(TIM1,ENABLE); //开启定时器
}
//设置比较值
void Set_compare(uint8_t Oc,int16_t count)
{
if(Oc == 1)
{
if(count<0)
{
count *= -1;
Left_Motor_turn_Negative;
}
else
{
Left_Motor_turn_Positive;
}
TIM_SetCompare1(TIM1,count);
}
if(Oc == 2)
{
if(count<0)
{
count *= -1;
Right_Motor_turn_Positive;
}
else
{
Right_Motor_turn_Negative;
}
TIM_SetCompare2(TIM1,count);
}
}
3.2、N20编码器减速电机(我的是6V300转)
编码器测速原理:电机转动时会产生A、B两相的脉冲信号,且这两路脉冲信号的相位差为90度(即正交),然后通过STM32定时器的编码器接口可接收增量(正交)编码器的信号,根据编码器旋转产生的正交信号脉冲,自动控制CNT自增或自减,从而指示编码器的位置、旋转方向和旋转速度。
脉冲次数计算方法:根据编码器的不同转动一圈产生的信号量也不同,以电机转动一圈产生7个信号,电机减速比为1:50为例子,那么转动一圈产生的脉冲信号为7*50=350个,再通过STM32的编码器接口的四倍频,也就是一圈会产生1400个脉冲。
具体接线:
红 | TB6612:AO2、BO1 |
白 | TB6612:AO1、BO2 |
黑 | 5V电源 |
蓝 | GND |
黄(A相) | MCU:PA6/PA7 |
绿(B相) | MCU:PB6/PB7 |
代码部分:
int16_t L_Temp;
int16_t R_Temp;
void Encoder_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_ICInitTypeDef TIM_ICInitStruct;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE); //L_Encoder
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE); //R_Encoder
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; //L:PA6 PA7||R:PB6 PB7
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStruct);
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 = 65536-1; //ARR
TIM_TimeBaseInitStruct.TIM_Prescaler = 1-1; //PSC
TIM_TimeBaseInitStruct.TIM_RepetitionCounter = 0; //关闭重复次数计数器
TIM_TimeBaseInit(TIM3,&TIM_TimeBaseInitStruct);
TIM_ICStructInit(&TIM_ICInitStruct); //结构体配置不完整时,默认给初始值
TIM_ICInitStruct.TIM_Channel = TIM_Channel_1; //L_A相
TIM_ICInitStruct.TIM_ICFilter = 0xF;; //滤波
TIM_ICInit(TIM3,&TIM_ICInitStruct);
TIM_ICInitStruct.TIM_Channel = TIM_Channel_2; //L_B相
TIM_ICInitStruct.TIM_ICFilter = 0xF;;
TIM_ICInit(TIM3,&TIM_ICInitStruct);
TIM_ICInitStruct.TIM_Channel = TIM_Channel_1;//R_A相
TIM_ICInitStruct.TIM_ICFilter = 0xF;
TIM_ICInit(TIM4,&TIM_ICInitStruct);
TIM_ICInitStruct.TIM_Channel = TIM_Channel_2;//R_B相
TIM_ICInitStruct.TIM_ICFilter = 0xF;
TIM_ICInit(TIM4,&TIM_ICInitStruct);
//使用编码器模式3
//参数一TIMx:定时器的选择
//参数二TIM_EncoderMode:编码器模式的选择
//参数三TIM_IC1Polarity:通道一的极性选择
//参数四TIM_IC2Polarity:通道二的极性选择
TIM_EncoderInterfaceConfig(TIM3,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
TIM_EncoderInterfaceConfig(TIM4,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
//清除TIM的更新标志位
TIM_ClearFlag(TIM3, TIM_FLAG_Update);
TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);
TIM_ClearFlag(TIM4, TIM_FLAG_Update);
TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE);
//复位计数器
TIM_SetCounter(TIM3,0);
TIM_SetCounter(TIM4,0);
TIM_Cmd(TIM3,ENABLE); //开启定时器
TIM_Cmd(TIM4,ENABLE);
}
int16_t Get_Left_Counter(void)
{
L_Temp = TIM_GetCounter(TIM3);
TIM_SetCounter(TIM3,0);
return L_Temp;
}
int16_t Get_Right_Counter(void)
{
R_Temp = TIM_GetCounter(TIM4);
TIM_SetCounter(TIM4,0);
return R_Temp;
}
3.3、MPU6050
简介:MPU6050是一款六轴传感器,包括三轴陀螺仪和三轴加速度计。它的原理是利用陀螺仪测量物体的旋转速度,加速度计测量物体的线性运动加速度。通过结合这两种数据,可以得到物体的运动状态,例如倾斜角度或者在空间中的方向。
具体接线:
VCC | 3.3V电源 |
GND | 地 |
SCL | PB10 |
SDA | PB11 |
从机地址AD0(默认低) | IIC地址为0X68(0),IIC地址为0X69(1) |
中断线INT | PB14 |
代码部分:
mpu_dmp_get_data(&pitch,&roll,&yaw); //获取姿态角
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //获取角速度
所需要的是这两个接口函数,由于代码过多,关于MPU6050的初始化、IIC通信驱动、DMP库获取姿态角可以自行搜索学习,本文不便展示。
二、软件设计
1、软件框图
2、PID算法
2.1、简介
PID 控制器(比例-积分-微分控制器)是一个在工业控制应用中常见的反馈回路部件,由比例单元 P、积分单元 I 和微分单元 D 组成。PID 控制的基础是比例控制;积分控制可消除稳态误差,但可能增加超调;微分控制可加快大惯性系统响应速度以及减弱超调趋势。具体原理可以自行搜索学习。
2.2、PID调参
直立环:
Kp极性:任意给定值后拿起小车前倾,车轮前转说明极性正确,如果极性错误前面加个负号即可
Kp大小:当给定值不断增大,当小车有低频振荡时说明Kp值为最大值,可引入Kd
Kd极性: Kp值设为0,任意给定Kd值后拿起小车绕电机轴旋转,车轮同相转动,说明极性正确
Kd大小: 恢复Kp值,Kd值不断增大,当小车有高频振荡是说明Kd值为最大值,可引入速度环
注意:直立环调完后两个系数乘0.6再调速度环
速度环:
Kp&Ki极性:手动转动一个车轮,另一个车轮会同向加速,直至最大说明极性正确
Kp&Ki大小:不断增加Kp&Ki值,直至小车保持平衡的同时,速度接近于零。且回位效果好
2.3、相关代码
uint8_t Run_Off(float Angle)
{
uint8_t off_flag;
if(Angle > 40 || Angle < -40)
{
off_flag = 1;
GPIO_ResetBits(GPIOA,GPIO_Pin_4),GPIO_ResetBits(GPIOA,GPIO_Pin_5);
GPIO_ResetBits(GPIOA,GPIO_Pin_0),GPIO_ResetBits(GPIOA,GPIO_Pin_1);
}
else off_flag = 0;
return off_flag;
}
void PID_Init()
{
/*直立PID环控制参数初始化*/
//P对应反应快慢
pid.balance_stand = 1.5;
pid.Kp_stand = -540;
pid.Kd_stand = -1.8;
/*速度PID环控制参数初始化*/
pid.Kp_speed = 115;
pid.Ki_speed = pid.Kp_speed / 200;
/*转向PID环控制参数初始化*/
pid.Kp_turn = 60;
pid.Kd_turn = 0;
}
/*
函数功能:直立环PD控制
入口参数:Angle 测量实际角度
返回 值:直立控制PWM
注 释:微分变量为直接读取的角速度
*/
extern short gyroy;
int balance(float Angle)
{
signed int Bias;
int balance;
Bias = Angle - pid.balance_stand; //角度误差
balance = (pid.Kp_stand * Bias) + (gyroy * pid.Kd_stand);
return balance;
}
/*
函数功能:速度环PI控制
入口参数:Encoder 左右电机转速
返回 值:速度控制PWM
*/
int velocity(int encoder_left,int encoder_right)
{
static float Velocity,Encoder_Err,Encoder_Last,Encoder_lowout,Movement;
static float Encoder_Integral;
static float Target_Velocity = 100;
if(Flag_Front == 1) Movement = Target_Velocity / 2;
else if(Flag_Rear == 1) Movement = -Target_Velocity / 2;
else if(Flag_Stop == 1) Movement = 0;
else Movement = 0;
Encoder_Err = (encoder_left+encoder_right) - 0; //获取最新速度偏差=测量速度(左右编码器之和)-目标速度(此处为零)
Encoder_lowout = 0.3 * Encoder_Err+0.7 * Encoder_Last;
Encoder_Last = Encoder_lowout;
Encoder_Integral += Encoder_lowout; //积分出位移 积分时间:10ms
Encoder_Integral = Encoder_Integral + Movement; //接收遥控器数据,控制前进后退
if(Encoder_Integral> 10000) Encoder_Integral = 10000; //积分限幅
if(Encoder_Integral<-10000) Encoder_Integral = -10000; //积分限幅
Velocity = Encoder_Err * pid.Kp_speed + Encoder_Integral * pid.Ki_speed;
if(Run_Off(pitch) == 1) Encoder_Integral = 0; //电机关闭,清除积分
return Velocity;
}
/*
函数功能:转向PD控制
入口参数:Z轴角速度
返回 值:转向控制PWM
*/
int turn(int16_t gyroz)
{
static float Turn_Target, Turn_Amplitude = 60;
int turn_out;
float Kp = pid.Kp_turn,Kd;
if(Flag_Left == 1) Turn_Target = Turn_Amplitude/2;
else if(Flag_Right ==1 ) Turn_Target = -Turn_Amplitude/2;
else if(Flag_Turn_Stop == 1) Turn_Target = 0;
else Turn_Target = 0;
if(1 == Flag_Front || 1 == Flag_Rear) Kd = pid.Kd_turn;
else Kd = 0;
turn_out = Turn_Target * Kp + gyroz * Kd;
return turn_out;
}
3、main.c代码
float pitch,roll,yaw; //欧拉角
int16_t aacx,aacy,aacz; //加速度原始数据
int16_t gyrox,gyroy,gyroz; //角速度原始数据
int8_t Flag_Front,Flag_Rear,Flag_Stop,Flag_Left,Flag_Right,Flag_Turn_Stop; //蓝牙遥控相关的变量
int16_t L_temp,R_temp,Moto1,Moto2; //陀螺仪原始数据 0
int balance_out = 0,velocity_out = 0,Turn_out = 0; //输出变量
uint8_t rxtest;
uint16_t j;
int main(void)
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
delay_init();
SystemInit();
Serial_TX_RX_Init(9600);
OLED_Init();
PID_Init();
PWM_Init();
Encoder_Init();
while(MPU_Init()!=0); //初始化MPU6050
while(mpu_dmp_init()!=0); //初始化DMP
Exti_Init(); //IN中断初始化
Timer_Init(); //100Hz
while(1)
{
OLED_ShowSignedNum(1,1,pitch,3);
OLED_ShowSignedNum(2,1,roll,3);
OLED_ShowSignedNum(3,1,yaw,3);
}
}
//得到MPU6050数据,进行pid算法设置pwm
void EXTI15_10_IRQHandler(void) //外部中断1(高优先级)
{
int PWM_out;
if(EXTI_GetITStatus(EXTI_Line14)==SET)
{
mpu_dmp_get_data(&pitch,&roll,&yaw); //获取姿态角
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //获取角速度
balance_out = balance(pitch); //直立环
velocity_out = velocity(L_temp,R_temp); //速度环
Turn_out = turn(gyroz); //转向环
PWM_out = balance_out + velocity_out;
Moto1 = PWM_out - Turn_out; //获得PWM输出值
Moto2 = PWM_out + Turn_out;
Moto1 = Limit_Pwm(Moto1,7200);
Moto2 = Limit_Pwm(Moto2,7200);
if(Run_Off(pitch) == 0)
{
Set_compare(1,Moto1);
Set_compare(2,Moto2);
}
EXTI_ClearITPendingBit(EXTI_Line14);
}
}
//蓝牙控制
void USART2_IRQHandler(void) //串口内部中断2(次高优先级)
{
if (USART_GetITStatus(USART2, USART_IT_RXNE) == SET) //标志位RXNE置1(RDR寄存器为非空)
{
//串口接收功能函数
uint8_t rxdata = USART_ReceiveData(USART2);
switch(rxdata)
{
case 'f': Flag_Front = 1,Flag_Rear = 0,Flag_Left = 0,Flag_Right = 0;//前
break;
case 'b': Flag_Front = 0,Flag_Rear = 1,Flag_Left = 0,Flag_Right = 0;//后
break;
case 'a': Flag_Front = 0,Flag_Rear = 0,Flag_Left = 1,Flag_Right = 0;//左
break;
case 'c': Flag_Front = 0,Flag_Rear = 0,Flag_Left = 0,Flag_Right = 1;//右
break;
case 's': Flag_Front = 0,Flag_Rear = 0,Flag_Left = 0,Flag_Right = 0,Flag_Stop = 1,Flag_Turn_Stop = 1;//停
break;
default :
break;
}
USART_ClearITPendingBit(USART2, USART_IT_RXNE); //清除RXNE位
}
}
//获取电机速度
void TIM2_IRQHandler(void) //100Hz定时器中断(低优先级)
{
if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET) //判断是否中断溢出
{
L_temp = Get_Left_Counter();
R_temp = -Get_Right_Counter();
TIM_ClearITPendingBit(TIM2, TIM_IT_Update); //清除中断标志位
}
}
三、代码资料
链接:百度网盘 请输入提取码
提取码:kax4