【STM32+HAL】巡逻打靶小车

南极熊ii2024-08-19 11:37

一、前言

作为电赛最爱出的小车和视觉题，将两者结合起来出题也是一个方向，故写下此文供学者参考，也作为备赛电赛的记录。

如有小伙伴想交流学习心得，欢迎加入群聊751950234，群内不定期更新代码，以及提供本人博客所有源码

二、题目分析

**实现目标：**当小车识别到黑块时，小车停止，开启激光打靶模式，并实现来回巡线打靶。
**思路：**为避免小车转头带来的不确定性，故本文采用前后各装一个光电传感器实现前进后退；OpenMV摄像头和激光装载在云台上用作打靶。

三、所用工具

1、芯片： STM32F407ZGT6

2、IDE： MDK-Keil软件

**3、库文件：**STM32F4xxHAL库

**4、视觉模块：**OpenMV4

**5、云台：**幻尔数字舵机云台

四、CubeMX配置

1、定时器配置

**TIM1：**马达控制定时器，控制周期1ms

**TIM6：**云台控制定时器，控制周期5ms

**TIM2：**电机控制PWM生成定时器

**TIM5：**云台控制PWM生成定时器

**TIM3、TIM4：**编码器定时器

有关定时器的相关配置，详见 【STM32+HAL】定时器功能小记

2、串口配置

**USART1：**单片机与电脑通讯

**USART2：**单片机与OpenMV通讯

至此，CubeMX配置完毕。

五、OpenMV识别

代码不难，大家自行理解。

有关更多OpenMV的内容，详见【OPENMV】学习记录 (持续更新)

python 复制代码 
import sensor, image, time, ustruct, math
from pyb import UART

uart = UART(3, 115200, timeout_char=200)
uart.init(115200, bits=8, parity=None, stop=1)  # 使用给定参数初始化

# 初始化摄像头
sensor.reset()                        # 初始化感应器
sensor.set_pixformat(sensor.RGB565)   # 设置像素格式为RGB565
sensor.set_framesize(sensor.QVGA)     # 设置帧大小为320x240
sensor.skip_frames(time = 2000)       # 跳过前2秒帧，用于摄像头设置稳定
sensor.set_auto_gain(False)           # 必须关闭才能进行颜色跟踪
sensor.set_auto_whitebal(False)       # 必须关闭才能进行颜色跟踪
clock = time.clock()                  # 初始化时钟对象

def find_circle(blobs):                # 寻找最圆色块
    max_circle = 1
    max_blob = None
    for blob in blobs:
        if blob.elongation() < max_circle:
            max_blob = blob
            max_circle = blob.elongation()
    return max_blob
def send_data(x, y):
    global uart
    uart.write(str(x))
    uart.write(bytearray([0x20]))
    uart.write(str(y))
    uart.write(bytearray([0x20]))

# 设置颜色阈值
threshold = (7, 54, 12, 63, 16, 69)

while(True):
    clock.tick()  # 开始新帧计时
    img = sensor.snapshot().gaussian(1,unsharp=True)  # 捕获图像
    blobs = img.find_blobs([threshold])
    if blobs:
        max_blob = find_circle(blobs)
        if max_blob:
            # 画出最大圆的中心和方向
            img.draw_keypoints([(max_blob.cx(), max_blob.cy(), int(math.degrees(max_blob.rotation())))], size=20, color=(255, 0, 0))
            img.draw_cross(max_blob.cx(), max_blob.cy(), color=(255, 0, 0))
            send_data(max_blob.cx(), max_blob.cy())
            # 打印出最大色块的中心位置和面积
            print(max_blob.cx(), max_blob.cy())

六、Keil填写代码

1、小车循迹函数

其中，P1~5 为车前光电传感器，Q1~5为车尾光电传感器。

cpp 复制代码 
/*=================== 循迹函数 ===================*/
int Track(void)
{
	int output=0;
	/* 前进差速 */
	if(flag == 1)
	{
		Speed_Middle  = 10;						//中值速度 10
		if(P2) output += 5;
		if(P4) output -= 5;
		if(P1) output += 8;
		if(P5) output -= 8;
	}
	/* 后退差速 */
	else if(flag == 2)
	{
		Speed_Middle  = -8;						//中值速度 -10
		if(Q2) output -= 10;
		else if(Q4) output += 10;
		if(Q1) output -= 15;
		else if(Q5) output += 15;
	}
	/* 直线匀速 */
	if((Q3 && Q2 == GPIO_PIN_RESET && Q4 == GPIO_PIN_RESET)
	 ||(P3 && P2 == GPIO_PIN_RESET && P4 == GPIO_PIN_RESET))
		output = 0;

	return output;
}
2、十字路口判定函数
cpp 复制代码 
/*=================== 十字路口判断函数 ===================*/
uint8_t Crossing(void)
{
	/* 前进档 */
	if(flag == 1)
	{
		if((P3 && P2 && P4))		//十字路口识别
		{
			Cross++;
			if(Cross >= 40)			//终点
			{
				Cross = 0;
				flag = 2;
				flag_temp = flag;
				return 1;
			}
			else if(Cross == 20)	//中点
			{
				flag_temp = flag;	//保存运动状态
				stop_cnt = 0;		//打靶时间
				flag = 0;			//暂停小车
				flag_stop = 1;		//开启云台
				return 1;
			}
		}
	}
	/* 倒挡 */
	else if(flag == 2)
	{
		if((Q3 && Q2 && Q4))
		{
			Cross++;
			if(Cross >= 45)
			{
				Cross = 0;
				flag = 1;
				flag_temp = flag;
				return 1;
			}
			else if(Cross == 20)	//中点
			{
				flag_temp = flag;	//保存运动状态
				stop_cnt = 0;		//打靶时间
				flag = 0;			//暂停小车
				flag_stop = 1;		//开启云台
				return 1;
			}
		}
	}
	return 0;
}
3、PID计算函数
cpp 复制代码 
/*=================== 增量式PID控制设计 ===================*/
//左A轮PID
float PID_A(float Encoder,float Target)
{
	static float Bias, Last_bias, Last2_bias, Pwm;
	Bias = Target - Encoder;
	Pwm += Kp1 * (Bias - Last_bias) + Ki1 * Bias + Kd1 * (Bias - 2 * Last_bias + Last2_bias);
	Last_bias = Bias;
	Last2_bias = Last_bias;
	return Pwm;
}

//右B轮PID
float PID_B(float Encoder,float Target)
{
	static float Bias, Last_bias, Last2_bias, Pwm;
	Bias = Target-Encoder;
	Pwm += Kp1 * (Bias - Last_bias) + Ki1 * Bias + Kd1 * (Bias - 2 * Last_bias + Last2_bias);
	Last_bias = Bias;
	Last2_bias = Last_bias;
	return Pwm;
}

//打靶PID_X
float PID_Target_X(float now,float target)
{
	static float Bias, Last_bias, Last2_bias, Pwm;
	Bias = target-now;
	Pwm += Kp2 * (Bias - Last_bias) + Ki2 * Bias + Kd2 * (Bias - 2 * Last_bias + Last2_bias);
	Last_bias = Bias;
	Last2_bias = Last_bias;
	return Pwm;
}

//打靶PID_Y
float PID_Target_Y(float now,float target)
{
	static float Bias, Last_bias, Last2_bias, Pwm;
	Bias = target-now;
	Pwm += Kp3 * (Bias - Last_bias) + Ki3 * Bias + Kd3 * (Bias - 2 * Last_bias + Last2_bias);
	Last_bias = Bias;
	Last2_bias = Last_bias;
	return Pwm;
}
4、电机云台控制总函数
cpp 复制代码 
/**************************************************************************
Function: Control function
Input   : none
Output  : none
函数功能：控制小车巡线
入口参数：无
返回  值：无
**************************************************************************/
void Control(void)
{
	if(flag == 0)									//暂停模式
		Motor_Left = 0, Motor_Right = 0, Cross = 0;
	else if(Crossing())								//识别到十字路口
		Motor_Left = 0, Motor_Right = 0;
	else											//普通巡线
	{
		CurrentA = (float)Read_Encoder(3);
		CurrentB = (float)Read_Encoder(4);
		TargetA = Speed_Middle + Track();
		TargetB = Speed_Middle - Track();

		Motor_Left  = (int)PWM_Limit(PID_A(CurrentA,TargetA), Limit, -Limit);
		Motor_Right = (int)PWM_Limit(PID_B(CurrentB,TargetB), Limit, -Limit);
	}
	Set_Pwm(Motor_Left, Motor_Right);
}



/**************************************************************************
Function: Target_Control
Input   : none
Output  : none
函数功能：控制激光云台
入口参数：无
返回  值：无
**************************************************************************/
void Target_Control(void)
{
	static int PWMX = 1500, PWMY = 1500;
	PWMX += PID_Target_X(110,Tx);
	PWMY += PID_Target_Y(100,Ty);
	PWMX = (PWMX < 1000) ? 1500 :((PWMX > 2200) ? 1500 : PWMX);
	PWMY = (PWMY < 1000) ? 1500: ((PWMY > 2200) ? 1500 : PWMY);
	PTZA = PWMX;
	PTZB = PWMY;
}
5、main.c

为缩减篇幅，下附代码删减了部分冗余的代码，相信以大家的聪明才智一定看得懂！

cpp 复制代码 
int main(void)
{
  /* USER CODE BEGIN 2 */
	/* OPENMV初始化 */
	__HAL_UART_ENABLE_IT(&huart2, UART_IT_IDLE);
	HAL_UART_Receive_DMA(&huart2,rx_buffer,RXBUFFERSIZE);
	HAL_Delay(100);

	/* 定时器初始化 */
	TIM_Init();
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */
    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}
/* USER CODE BEGIN 4 */
/*=================== 定时器中断 ===================*/
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
	if(flag == 0){
		Set_Pwm(0,0);					//马达停转
		if(htim -> Instance == TIM6)
		{
			if(flag_stop) 				//到中点后暂停，开始识别
			{
				stop_cnt++;
				if(stop_cnt >= 150)		//暂停一段时间后
				{
					flag = flag_temp;	//恢复停止前状态
					stop_cnt = 0;
				}
			}
			Target_Control();
		}
	}
	else
	{
		if (htim -> Instance == TIM1){
			Control();
			PTZA = 1500;				//云台复位
			PTZB = 1500;
		}
	}
}


/*=================== 定时器初始化 ===================*/
void TIM_Init(void)
{
	/* 使能编码器输出 */
	HAL_TIM_Encoder_Start(&htim3,TIM_CHANNEL_ALL);
	HAL_TIM_Encoder_Start(&htim4,TIM_CHANNEL_ALL);

	/* 失能所有输出 */
	AIN10;AIN20;BIN10;BIN20;
	TIM2->CCR1 = 0;
	TIM2->CCR2 = 0;
	TIM5->CCR2 = 1500;
	TIM5->CCR4 = 1500;

	/* 开启PWM波 */
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(&htim5,TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(&htim5,TIM_CHANNEL_4);

	HAL_Delay(1000);
	/* 开启控制中断 */
	HAL_TIM_Base_Start_IT(&htim1);
	HAL_TIM_Base_Start_IT(&htim6);
}


/*=================== 按键切换模式 ===================*/
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)  
{
	if(HAL_GPIO_ReadPin(WKUP_GPIO_Port,WKUP_Pin) == GPIO_PIN_SET){
		HAL_Delay(20); //延时消抖
		if(HAL_GPIO_ReadPin(WKUP_GPIO_Port,WKUP_Pin) == GPIO_PIN_SET){
			HAL_GPIO_TogglePin(LED_GPIO_Port,LED_Pin);
			flag = (flag + 1) % 3;						//按键切换模式
		}
	}
}
/* USER CODE END 4 */

七、源码提供

夸克网盘：我用夸克网盘分享了「WheeleCar」，点击链接即可保存。

百度网盘：通过百度网盘分享的文件：WheeleCar 提取码：6666

Gitee：WheeleCar

CSDN：WheeleCar

八、结语

本人能力有限，代码未必是最优解，若有可改进之处望在评论区留言。

相关推荐
redcocal10 小时前
地平线秋招
python·嵌入式硬件·算法·fpga开发·求职招聘
辰哥单片机设计13 小时前
门磁模块详解（防盗感应开关 STM32）
stm32·单片机·嵌入式硬件·传感器
夜间去看海13 小时前
基于51单片机的自动清洗系统（自动洗衣机）
嵌入式硬件·51单片机·proteus·洗衣机
yrx02030714 小时前
stm32 IIC总线busy解决方法
stm32·单片机·嵌入式硬件
YHPsophie15 小时前
ATGM331C-5T杭州中科微BDS/GNSS全星座定位授时模块应用领域
经验分享·笔记·单片机·信息与通信·交通物流
Archie_IT16 小时前
【STM32系统】基于STM32设计的SD卡数据读取与上位机显示系统（SDIO接口驱动、雷龙SD卡）——文末资料下载
arm开发·stm32·单片机·嵌入式硬件
辰哥单片机设计16 小时前
1×4矩阵键盘详解（STM32）
stm32·单片机·嵌入式硬件·矩阵·传感器
wmkswd16 小时前
CAN总线-STM32上CAN外设
stm32·单片机·嵌入式硬件
Ruohongxu16 小时前
LAN8720A-CP-TR-ABC QFN-24 以太网收发器芯片
单片机
热门推荐
01RAG 实践- Ollama+RagFlow 部署本地知识库02组基轨迹建模 GBTM的介绍与实现（Stata 或 R）032024年高教社杯数学建模国赛C题超详细解题思路分析04Coze扣子平台完整体验和实践（附国内和国际版对比）05苍穹外卖面试总结06【2024数模国赛赛题思路公开】国赛B题思路丨附可运行代码丨无偿自提07yolov8实战第五天——yolov8+ffmpg实时视频流检测并进行实时推流——（推流，保姆教学）08CCF-CSP认证考试 202406-3 文本分词 100分题解09【2024高教社杯全国大学生数学建模竞赛】B题 生产过程中的决策问题——解题思路 代码 论文10浏览器插件——ModHeader 的分享和学习