一、前言
作为电赛最爱出的小车和视觉题，将两者结合起来出题也是一个方向，故写下此文供学者参考，也作为备赛电赛的记录。
如有小伙伴想交流学习心得，欢迎加入群聊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, 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为车尾光电传感器。
/*=================== 循迹函数 ===================*/
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、十字路口判定函数
/*=================== 十字路口判断函数 ===================*/
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轮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、电机云台控制总函数
/**************************************************************************
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
为缩减篇幅，下附代码删减了部分冗余的代码，相信以大家的聪明才智一定看得懂！
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
八、结语
本人能力有限，代码未必是最优解，若有可改进之处望在评论区留言。
