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

前言

碎碎念一下,本篇博客为个人项目总结,因技术一般且记性较差(笑)故写此博客以供记录与复盘

硬件

硬件准备

模块型号数量碎碎念
主控极海APM32VBT61因原主控指南者VET6故障,中途换成国产板子
电源DC头锂电池1注意电池供电头应与DC/DC降压模块的口一致
DC/DC降压模块XY-36061注意与锂电池的适配性
电机驱动TB66122建议多买几个,TB6612容易炸,一炸等物流就要两三天,一定不要短路!!严重了直接板子电脑主板一块带走
电机+轮子套件JGB37-520霍尔编码器电机2注意,电机架要能固定到小车板子上,最好买带编码器的,否则不能做后续的PID闭环控制
红外循迹传感器TCRT50006+越多越好,反正便宜,只有传感器数量上去了,后续PID循迹时小车速度才能上去,才能更稳
蓝牙透传模块HC-051-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);
	}
} 

  • 20
    点赞
  • 6
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值