【STM32+OPENMV】二维云台颜色识别及追踪

南极熊ii

已于 2024-10-16 18:33:27 修改

阅读量3.5k

点赞数 15

分类专栏：电赛文章标签： stm32 嵌入式硬件单片机

于 2024-03-05 22:14:24 首次发布

本文链接：https://blog.csdn.net/m0_60372743/article/details/136491052

版权

电赛专栏收录该内容

8 篇文章 4 订阅

订阅专栏

本文详细描述了使用STM32F407ZGT6开发板，通过CUBEMX和KEIL5进行编程，实现OpenMV驱动的二维云台PID追踪功能，并将追踪结果显示在七针OLED屏上，包括HAL配置、PWM舵机控制和PID算法应用。

摘要由CSDN通过智能技术生成

一、准备工作

有关OPENMV最大色块追踪及与STM32通信内容，详见【STM32+HAL】与OpenMV通信

有关串口DMA传输的内容，详见【STM32+HAL】DMA应用

二、软件工具

1、芯片： STM32F103C8T6

2、IDE： MDK-Keil软件

3、库文件：STM32F1xxHAL库

三、实现功能

OpenMV识别追踪物体，并将坐标发送至32主控端，32控制二维云台进行追踪，并打印x,y坐标。

四、CubeMX配置

1、生成两路PWM波控制舵机

一路控制X轴，一路控制Y轴。

频率frequency = 72MHz / 24 / 60000 = 50Hz。

2、控制定时器

控制周期设置为1ms，即每隔1ms对舵机发出控制指令

3、配置中断

配法不唯一，逻辑正确即可

五、OpenMV识别代码

前言引用的链接文章已做详细介绍，这里不再赘述，代码简介逻辑清晰，各位自行理解！

import time
import sensor
import math
import image
import ustruct
from pyb import UART


uart = UART(3, 115200, timeout_char=200)
uart.init(115200, bits=8, parity=None, stop=1)  # init with given parameters

threshold_index = 1                             #索引
thresholds = [                                  #阈值数组
    (7, 13, -18, -3, 4, 12),
    (15, 77, 13, 52, 20, 44),
]

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)# QVGA的中心坐标：160,120
sensor.skip_frames(time=2000)    # 跳过2000毫秒的帧让相机图像在改变相机设置后稳定下来
sensor.set_auto_gain(False)      # 必须关闭才能进行颜色跟踪
sensor.set_auto_whitebal(False)  # 必须关闭才能进行颜色跟踪
clock = time.clock()

#寻找最大色块函数
def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob.pixels() > max_size:
            max_blob = blob
            max_size = blob.pixels()
    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]))


while True:
    clock.tick()
    img = sensor.snapshot().lens_corr(1.8)  #摄像头畸变矫正
    blobs = img.find_blobs([thresholds[threshold_index]])

    #如果找到了目标颜色
    if blobs:
        max_blob = find_max(blobs)
        cx=max_blob[5]
        cy=max_blob[6]

        # 这些值始终是稳定的。
        img.draw_rectangle(max_blob.rect(),(0,0,255))
        img.draw_cross(cx, cy,(255,0,0))

        send_data(cx,cy)      # 发送数据

六、Keil填写代码

1、stm32f1xx_it.c

串口DMA接收OpenMV数据函数，使用DMA接收不仅效率高，而且对数据的解析方式也更简单。想看更多DMA应用的详见【STM32+HAL】DMA应用

这里仅做串口DMA的简单使用教学，为缩小篇幅，下附代码并非stm32f1xx_it.c的全部代码，只是需要在这几个代码块中添加代码，望读者自行寻找添加。

串口DMA传输：USART2_IRQHandler为中断接收函数，每当USART2接收到数据后就会进入此函数进行数据解析判断，不需要经过CPU处理，大大提高了程序运行效率。

经过此函数后，接收到的字符串就会通过sscanf((const char *)rx_buffer, "%d %d", &cx, &cy); 这句代码将OpenMV端发送的包含两个整型数的字符串提取出来，存储到cx，cy两个变量中。

/* USER CODE BEGIN Includes */
#include "stdio.h"
/* USER CODE END Includes */

/* USER CODE BEGIN PD */
#define RXBUFFERSIZE  256
/* USER CODE END PD */

/* USER CODE BEGIN PV */
extern uint8_t rx_buffer[RXBUFFERSIZE];		//接收数组
extern volatile uint8_t rx_len; 			//接收到的数据长度
extern volatile uint8_t recv_end_flag; 		//接收结束标志位

extern int cx, cy;
/* USER CODE END PV */


void USART2_IRQHandler(void)
{
  /* USER CODE BEGIN USART2_IRQn 0 */

  /* USER CODE END USART2_IRQn 0 */
  HAL_UART_IRQHandler(&huart2);
  /* USER CODE BEGIN USART2_IRQn 1 */
	uint8_t tmp_flag =__HAL_UART_GET_FLAG(&huart2,UART_FLAG_IDLE); 	//获取IDLE标志位
	if((tmp_flag != RESET))											//通过标志位判断接收是否结束
	{
		recv_end_flag = 1; 											//置1表明接收结束
		__HAL_UART_CLEAR_IDLEFLAG(&huart2);							//清除标志位
		HAL_UART_DMAStop(&huart2);
		uint8_t temp = __HAL_DMA_GET_COUNTER(&hdma_usart2_rx);
		rx_len = RXBUFFERSIZE - temp; 								//计算出数据长度
		sscanf((const char *)rx_buffer, "%d %d", &cx, &cy);			//字符转换
		HAL_UART_Receive_DMA(&huart2, rx_buffer, RXBUFFERSIZE);		//开启DMA接收，方便下一次接收数据
	}
  /* USER CODE END USART2_IRQn 1 */
}

2、ptz.c

关键控制函数

void Tilt(void)：总控制函数，将双向PID计算返回值限幅后，对定时器的CCR进行赋值。

int PID1(int current,int target)：计算当前值与目标值的差值，并通过改变PID参数值计算所需的PWM占空比，实现最基础的PID函数功能。

#include "ptz.h"
#include "main.h"
#include "tim.h"

#define CCR_UD 			TIM1->CCR1   	//up and down....		RANGE:1250-7500
#define CCR_LR 			TIM1->CCR2   	//Left and Right....	RANGE:1250-7500
#define Cen_x  			160				//x轴中心坐标值
#define Cen_y  			120				//y轴中心坐标值

#define KP1 			0.45
#define KD1 			2
#define KP2 			0.35
#define KD2 			2

#define sp1				30
#define sp2				23
#define range			35

int cx = 0, cy = 0;


/*
	* @brief:		PTZ control function
	* @param:		None
	* @retval:		None
*/
void Tilt(void)
{
	if(PID1(cx, Cen_x) + CCR_LR > 7450) CCR_LR = 7450;	//Right limit
	else if(CCR_LR < 1250) CCR_LR = 1250;				//Left limit
	else CCR_LR += PID1(cx, Cen_x);	

	if(CCR_UD> 4000) CCR_UD = 4000;						//Up limit
	else if(CCR_UD< 1250) CCR_UD = 1250;				//Down limit
	else CCR_UD -= PID2(cy, Cen_y);
}



/*
	* @brief:		PID control function
	* @param:		current:current value;target:target value
	* @retval:		None
*/
int PID1(int current,int target)  			//PID速度控制
{
	static int LastError;					//Error[-1]
	int iError,Outpid;						//当前误差

	iError = target - current;		 		//增量计算
	Outpid = (KP1 * iError)  				//E[k]项
			+(KD1 * (iError-LastError));	//E[k]-E[k-1]项
	LastError = iError;						//存储误差，用于下次计算
	return Outpid;
}

int PID2(int current,int target)  			//PID速度控制
{
	static int LastError;					//Error[-1]
	int iError,Outpid;						//当前误差

	iError = target - current;		 		//增量计算
	Outpid = (KP2 * iError)  				//E[k]项
			+(KD2 * (iError - LastError));	//E[k]-E[k-1]项
	LastError = iError;						//存储误差，用于下次计算
	return Outpid;
}

3、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);	

	/* PWM初始化 */
	TIM1->CCR1 = 1250-1;
	TIM1->CCR2 = 4400-1;
	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);

	/* 定时器中断开启 */
	HAL_TIM_Base_Start_IT(&htim2);
  /* 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 ( htim -> Instance == TIM2 ){
		 Tilt();
	}
}