【STM32 HAL库】寻迹小车开环控制状态机 TB6612+TCRT5000+HC-05

前言
硬件
- 硬件准备
- 模块说明
- - 主控 APM32F103VBT6核心板
  - DC/DC降压模块
  - 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);
	}
}