【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, 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

八、结语

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