【STM32+HAL】巡逻打靶小车

一、前言

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

如有小伙伴想交流学习心得，欢迎加入群聊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】学习记录 (持续更新)

import sensor, image, time, ustruct, mathfrom pyb import UARTuart = UART(3, 115200, timeout_char=200)uart.init(115200, bits=8, parity=None, stop=1) # 使用给定参数初始化# 初始化摄像头sensor.reset() # 初始化感应器sensor.set_pixformat(sensor.RGB565) # 设置像素格式为RGB565sensor.set_framesize(sensor.QVGA) # 设置帧大小为320x240sensor.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_blobdef 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为车尾光电传感器。

/*=================== 循迹函数 ===================*/int Track(void){int output=0;/* 前进差速 */if(flag == 1){Speed_Middle = 10;//中值速度 10if(P2) output += 5;if(P4) output -= 5;if(P1) output += 8;if(P5) output -= 8;}/* 后退差速 */else if(flag == 2){Speed_Middle = -8;//中值速度 -10if(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、十字路口判定函数

/*=================== 十字路口判断函数 ===================*/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计算函数

/*=================== 增量式PID控制设计 ===================*///左A轮PIDfloat 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轮PIDfloat 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_Xfloat 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_Yfloat 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、电机云台控制总函数

/**************************************************************************Function: Control functionInput : noneOutput : 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_ControlInput : noneOutput : 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  2200) ? 1500 : PWMX);PWMY = (PWMY  2200) ? 1500 : PWMY);PTZA = PWMX;PTZB = PWMY;}

5、main.c

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

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

八、结语

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