【STM32 HAL库】寻迹小车 开环控制 状态机 TB6612+TCRT5000+HC-05
前言
碎碎念一下,本篇博客为个人项目总结,因技术一般且记性较差(笑)故写此博客以供记录与复盘
硬件
硬件准备
模块 | 型号 | 数量 | 碎碎念 |
---|---|---|---|
主控 | 极海APM32VBT6 | 1 | 因原主控指南者VET6故障,中途换成国产板子 |
电源 | DC头锂电池 | 1 | 注意电池供电头应与DC/DC降压模块的口一致 |
DC/DC降压模块 | XY-3606 | 1 | 注意与锂电池的适配性 |
电机驱动 | TB6612 | 2 | 建议多买几个,TB6612容易炸,一炸等物流就要两三天,一定不要短路!!严重了直接板子电脑主板一块带走 |
电机+轮子套件 | JGB37-520霍尔编码器电机 | 2 | 注意,电机架要能固定到小车板子上,最好买带编码器的,否则不能做后续的PID闭环控制 |
红外循迹传感器 | TCRT5000 | 6+ | 越多越好,反正便宜,只有传感器数量上去了,后续PID循迹时小车速度才能上去,才能更稳 |
蓝牙透传模块 | HC-05 | 1-2 | 建议备用一个 |
小车套件 | 酷点机器人小车套件 | 1 | 买哪家的都可,注意最好孔位多,要能适配你的板子以及好安装其他模块 |
耗材 | 杜邦线,转接线等等 | 充足 | 注意及时补充 |
模块说明
主控 APM32F103VBT6核心板
优点
啊这,实际上是手头没板子了,只能先用这个过度下
缺点
1.板载资源较少(4个定时器,不足以完成驱动电机以及编码器测速的功能
2.国产平替的板子再怎么说兼容性都不能100%等于STM32,且开源资料有限,谨慎考虑
建议
建议买个资源多点的板子,建议vet6起步,硬件资源有限真的有点难绷
DC/DC降压模块
功能
将锂电池输入进DCDC的12v转换为12v与5v输出
TB6612电机驱动
功能
本质上讲,TB6612是一个电子开关,它根据接收到的PWM信号,来控制”开“与”关“时间的比例,根据“占空比”,输出特定的电压,以此电压来驱动电机
TCRT5000红外循迹模块
功能
发射红外光到反射面,若反射面吸收,则接收不到反射回的红外光,则led熄灭,D0口输出高电平
所以在合适的阈值下,根据D0口的电平高低,就能判断处是否检测到黑线(黑线吸收红外线)
HC-05蓝牙透传模块
功能
将复杂的蓝牙协议简化为串口透传,本质上就是无线的串口通信
代码逻辑
宏观框架
状态机
状态机图
状态机伪代码
void fsm(void)
{
switch(当前状态)
{
case 空闲状态:
{
//电机停止
switch(当前事件)
{
case 空闲事件:
当前状态 = 空闲状态;
break;
case 循迹事件:
当前状态 = 循迹状态;
break;
default:
当前状态 = 运动状态;
break;
}
}
break;
case 循迹状态:
{
//循迹
switch(当前事件)
{
case 空闲事件:
当前状态 = 空闲状态;
break;
case 循迹事件:
当前状态 = 循迹状态;
break;
default:
当前状态 = 运动状态;
break;
}
}
break;
case 运动状态:
{
switch(当前事件)
{
case 空闲事件:
当前状态 = 空闲状态;
break;
case 循迹事件:
当前状态 = 循迹状态;
break;
case 直走事件:
//直走
break;
case 后退事件:
//后退
break;
case 左转事件:
//左转
break;
case 右转事件:
//右转
break;
case 加速事件:
//加速
break;
case 减速事件:
//减速
break;
case 速度最大事件:
//速度最大
break;
case 停止事件:
//停止
break;
}
}
break;
}
}
状态机代码
void fsm(void)
{
switch(cur_state)
{
case S0_IDLE:
{
stop();
switch(EvntID)
{
case E0_IDLE:
cur_state = S0_IDLE;
break;
case E1_TRACK:
cur_state = S1_TRACK;
break;
default:
cur_state = S2_SPORT;
break;
}
}
break;
case S1_TRACK:
{
track();
switch(EvntID)
{
case E0_IDLE:
cur_state = S0_IDLE;
break;
case E1_TRACK:
cur_state = S1_TRACK;
break;
default:
cur_state = S2_SPORT;
break;
}
}
break;
case S2_SPORT:
{
switch(EvntID)
{
case E0_IDLE:
cur_state = S0_IDLE;
break;
case E1_TRACK:
cur_state = S1_TRACK;
break;
case E2_GO:
go1();
break;
case E3_BACK:
back();
break;
case E4_LEFT:
left();
break;
case E5_RIGHT:
right();
break;
case E6_SPEED_UP:
speed_up();
break;
case E7_SPEED_DOWN:
speed_down();
break;
case E8_SPEED_MAX:
speed_max();
break;
case E9_STOP:
stop();
break;
}
}
break;
}
}
框架测试
以HC-05蓝牙透传模块控制小车为例
HC-05初始化
void hc05_init(void)
{
HAL_UART_Receive_IT(&huart3, &receiveData,1);
}
接收完成中断回调函数中实现“当前事件”的更新,从而实现状态机的切换
/*
简述:重定义接收完成中断回调函数
详解:根据接收到蓝牙调试助手发送的数据,更新事件(以供状态机切换
*/
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
if(huart == &huart3)
{
//用以调试(判断是否进入中断,判断当前receiveData值
// printf("OK\n");
// printf("receiveData = %d\n",receiveData-48);
switch(receiveData-48)
{
case 0:
EvntID = E0_IDLE;
printf("STOP\n");
break;
case 1:
EvntID = E1_TRACK;
printf("TRACK\n");
break;
case 2:
EvntID = E2_GO;
printf("GO\n");
break;
case 3:
EvntID = E3_BACK;
printf("BACK\n");
break;
case 4:
EvntID = E4_LEFT;
printf("LEFT\n");
break;
case 5:
EvntID = E5_RIGHT;
printf("RIGHT\n");
break;
case 6:
EvntID = E6_SPEED_UP;
printf("SPEED_UP\n");
break;
case 7:
EvntID = E7_SPEED_DOWN;
printf("SPEED_DOWN\n");
break;
case 8:
EvntID = E8_SPEED_MAX;
printf("SPEED_MAX\n");
break;
case 9:
EvntID = E9_STOP;
printf("STOP\n");
break;
default:
EvntID = E0_IDLE; // 默认情况下返回到空闲状态
printf("ERROR_STOP\n");
break;
}
HAL_UART_Receive_IT(&huart3, &receiveData, 1);
}
}
微观模块
也就是状态机伪代码中的基本的功能代码
电机模块
motor.c
#include "motor.h"
uint16_t pulse_l,pulse_r;
float speed_l,speed_r;
/*
简述:启动电机函数
详解:开启TIM定时器的PWM模式,开始产生PWM波,启动电机
*/
void Motor_Start(void)
{
//启动左侧A相电机
HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_1);
//启动右侧B相电机
HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_4);
}
/*
简述:设置小车速度
详解:根据PWM占空比与小车速度关系,改变PWM比较寄存器值,从而改变占空比控制小车速度
*/
void Motor_SetSpeed(MotorDirection Mode,float speed_l,float speed_r)
{
if(0 <= speed_l && speed_l <= 356)
pulse_l = 1000 - 2.8086*speed_l;
if(0 <= speed_r && speed_r <= 356)
pulse_r = 1000 - 2.8086*speed_r;
if(Mode == FORWARD)
{
HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_RESET);
HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,GPIO_PIN_SET);
HAL_GPIO_WritePin(BIN1_GPIO_Port,BIN1_Pin,GPIO_PIN_RESET);
HAL_GPIO_WritePin(BIN2_GPIO_Port,BIN2_Pin,GPIO_PIN_SET);
}
else if(Mode == BACKWARD)
{
HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_SET);
HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,GPIO_PIN_RESET);
HAL_GPIO_WritePin(BIN1_GPIO_Port,BIN1_Pin,GPIO_PIN_SET);
HAL_GPIO_WritePin(BIN2_GPIO_Port,BIN2_Pin,GPIO_PIN_RESET);
}
__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_1,pulse_l);
__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_4,pulse_r);
}
/*
简述:小车基本运动模式
详解:设置小车左右轮速度,从而改变小车运动方式
*/
void go(void)
{
Motor_SetSpeed(FORWARD,52,50);
}
void go1(void)
{
Motor_SetSpeed(FORWARD,100,95);
}
void stop(void)
{
Motor_SetSpeed(FORWARD,5,5);
}
void back(void)
{
Motor_SetSpeed(BACKWARD,100,100);
}
void left(void)
{
Motor_SetSpeed(FORWARD,1,70);
}
void left1(void)
{
Motor_SetSpeed(FORWARD,5,50);
}
void left2(void)
{
Motor_SetSpeed(FORWARD,5,150);
}
void left3(void)
{
Motor_SetSpeed(FORWARD,5,200);
}
void right(void)
{
Motor_SetSpeed(FORWARD,70,1);
}
void right1(void)
{
Motor_SetSpeed(FORWARD,50,5);
}
void right2(void)
{
Motor_SetSpeed(FORWARD,150,5);
}
void right3(void)
{
Motor_SetSpeed(FORWARD,200,5);
}
/*
简述:小车加速函数
详解:获取小车1、2相电机对应的定时器的比较寄存器的当前值,并定义最小比较寄存器值,用以控制小车加速后速度上限
*/
void speed_up(void)
{
uint16_t motor_a_compare,motor_b_compare;
uint16_t min_compare = 700;
uint16_t max_change_i;
motor_a_compare = __HAL_TIM_GET_COMPARE(&htim4,TIM_CHANNEL_1);
motor_b_compare = __HAL_TIM_GET_COMPARE(&htim4,TIM_CHANNEL_4);
if(motor_a_compare>motor_b_compare)
max_change_i = motor_b_compare - min_compare;
else
max_change_i = motor_a_compare - min_compare;
for(float i = 0;i <= max_change_i;i+=0.001)
{
__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_1,motor_a_compare - i);
__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_4,motor_b_compare - i);
}
}
/*
简述:小车减速函数
详解:获取小车1、2相电机对应的定时器的比较寄存器的当前值,并定义最大比较寄存器值,用以控制小车减速后速度下限
*/
void speed_down(void)
{
uint16_t motor_a_compare,motor_b_compare;
uint16_t max_compare = 999;
uint16_t max_change_i;
motor_a_compare = __HAL_TIM_GET_COMPARE(&htim4,TIM_CHANNEL_1);
motor_b_compare = __HAL_TIM_GET_COMPARE(&htim4,TIM_CHANNEL_4);
if(motor_a_compare>motor_b_compare)
max_change_i = max_compare - motor_a_compare;
else
max_change_i = max_compare - motor_b_compare;
for(float i = 0;i <= max_change_i;i+=0.001)
{
__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_1,motor_a_compare + i);
__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_4,motor_b_compare + i);
}
}
/*
简述:小车加速到最大速度函数
详解:for循环中比较寄存器值自减,实现小车速度自增(设置并控制速度上限(减速比较小,最大速度过大))
*/
void speed_max(void)
{
uint16_t max_i = 270;
static float i = 0;
for(;i <= max_i;i+=0.001)
{
__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_1,1000 - i);
__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_4,1000 - i);
}
}
循迹模块
tcrt5000.c
#include "tcrt5000.h"
DIRECTION mode;
DIRECTION LEFTMAX;
DIRECTION RIGHTMAX;
/*
简述:判断当前位置状态
详解:根据红外传感器的D0值-->判断当前小车偏移量(LEFT1左一级偏移,LEFT2左二级偏移以此类推)
基本逻辑:先判断左右侧感应到黑线的最远端传感器,若左侧没感应到黑线,则以右侧感应到黑线的最远端传感器作为偏移程度;若右侧没感应到黑线则同理;若两侧都没感应到黑线或都感应到黑线,则直行
举例:若左1左2感应到黑线,左3没感应到黑线,则左MAX=左2。若右1右2右3都没感应到黑线,则右MAX=GO。此时偏移量mode=左MAX=左2,在循迹处左转程度为LEFT2对应的左转程度
*/
void GetDirection(void)
{
//判断左侧传感器中感应到黑线的最远端传感器
if(L3 == 1)
LEFTMAX = LEFT3;
else if(L2 == 1)
LEFTMAX = LEFT2;
else if(L1 == 1)
LEFTMAX = LEFT1;
else
LEFTMAX = GO;
//判断右侧传感器中感应到黑线的最远端传感器
if(R3 == 1)
RIGHTMAX = RIGHT3;
else if(R2 == 1)
RIGHTMAX = RIGHT2;
else if(R1 == 1)
RIGHTMAX = RIGHT1;
else
RIGHTMAX = GO;
//判断偏移量
if((LEFTMAX == GO) && (RIGHTMAX == GO))
mode = GO;
else if((LEFTMAX != GO) && (RIGHTMAX == GO))
mode = LEFTMAX;
else if((LEFTMAX == GO) && (RIGHTMAX != GO))
mode = RIGHTMAX;
else
mode = GO;
}
/*
简述:循迹函数
详解:先更新当前位置状态,根据当前小车偏移量执行对应纠正偏移的动作
*/
void track(void)
{
//先更新当前偏移量
GetDirection();
//根据偏移量执行循迹操作(偏移量越大,转弯程度越大
switch(mode)
{
case GO:
go();
break;
case LEFT1:
left1();
break;
case LEFT2:
left2();
break;
case LEFT3:
left3();
break;
case RIGHT1:
right1();
break;
case RIGHT2:
right2();
break;
case RIGHT3:
right3();
break;
}
}
蓝牙控制模块
#include "hc05.h"
uint8_t receiveData;
/*
简述:hc05蓝牙透传模块初始化函数
详解:开启中断接收
*/
void hc05_init(void)
{HAL_UART_Receive_IT(&huart3, &receiveData,1);}
/*
简述:重定义接收完成中断回调函数
详解:根据接收到蓝牙调试助手发送的数据,更新事件(以供状态机切换
*/
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
if(huart == &huart3)
{
// 用以调试(判断是否进入中断,判断当前receiveData值
// printf("OK\n");
// printf("receiveData = %d\n",receiveData-48);
switch(receiveData-48)
{
case 0:
EvntID = E0_IDLE;
printf("STOP\n");
break;
case 1:
EvntID = E1_TRACK;
printf("TRACK\n");
break;
case 2:
EvntID = E2_GO;
printf("GO\n");
break;
case 3:
EvntID = E3_BACK;
printf("BACK\n");
break;
case 4:
EvntID = E4_LEFT;
printf("LEFT\n");
break;
case 5:
EvntID = E5_RIGHT;
printf("RIGHT\n");
break;
case 6:
EvntID = E6_SPEED_UP;
printf("SPEED_UP\n");
break;
case 7:
EvntID = E7_SPEED_DOWN;
printf("SPEED_DOWN\n");
break;
case 8:
EvntID = E8_SPEED_MAX;
printf("SPEED_MAX\n");
break;
case 9:
EvntID = E9_STOP;
printf("STOP\n");
break;
default:
EvntID = E0_IDLE; // 默认情况下返回到空闲状态
printf("ERROR_STOP\n");
break;
}
HAL_UART_Receive_IT(&huart3, &receiveData, 1);
}
}