基于STM32与K210的云台追踪装置

Auroraisme

已于 2023-09-30 22:43:37 修改

阅读量2.4k

点赞数 6

文章标签： stm32 嵌入式硬件单片机 python

于 2023-09-30 22:38:56 首次发布

本文链接：https://blog.csdn.net/qq_64206165/article/details/133442842

版权

一、摘要

本文通过K210作为是识别模块去识别被测物体（以红色小球为例），当其识别到红色小球后，判断小球中心点所在的区域信息，并将其区域标志位通过串口发送给STM32，当STM32接收到位置信息后对x轴、y轴的两个舵机参数进行操作，最后通过定时器输出合适的PWM波，控制舵机旋转相应的角度，使K210摄像头对准被测物体，以实现物体追踪功能。

二、K210端

1.摄像头配置

sensor.set_pixformat(sensor.RGB565)#设置为彩色
sensor.set_framesize(sensor.QVGA)#设置分辨率，即图像大小
sensor.skip_frames(time = 100)
#sensor.set_auto_gain(False)
#sensor.set_auto_whitebal(False)
sensor.set_vflip(True)#垂直方向翻转

2.颜色预处理

寻找最大色块的函数：

#寻找最大色块
def find_max(blobs):
    max_size=0
    max_blob=0
    for blob in blobs:
        if  blob.pixels() > max_size:
            max_blob=blob
            max_size = blob.pixels()
    return max_blob

在追踪识别的过程中，可能出现背景或是其他区域出现小面积的红色区域，这会对识别造成影响，所以需要用程序过滤掉那些小的红色区域。

颜色阈值可通过IDE右侧的帧缓冲区框选要识别的物体如图所示：

框取红色物块后下面会显示其LAB的最大值和最小值，将thresholds列表修改即可

3.串口通信（发送）

from machine import UART
from fpioa_manager import fm # GPIO重定向函数

fm.register(18, fm.fpioa.UART1_TX, force=True)
uart_A = UART(UART.UART1,9600, 8, 0, 1, timeout=1000, read_buf_len=4096)

#串口打包发送函数
def sending_data(x,y):
    FH = bytearray([0x2C,0x12,x,y,0x5B])#拼字节
    uart_A.write(FH);

sending_data(cx,cy)#发送cx，cy变量

注册一个GPIO口配置为UART1的TX，波特率为9600，将要发送的数据打包为字符串的形式进行发送，数据包除要发送的变量外，还有帧头（0x2C、0x12）和帧尾（0x5B）.

4.寻找色块

 img = sensor.snapshot()#拍摄一张照片，img为一个image对象
    blobs=img.find_blobs([thresholds[0]], pixels_threshold=100, area_threshold=100, merge=True, margin=10)
    max_b=find_max(blobs)#寻找最大色块
    cx=0;cy=0;
#用for循环把所有的色块找一遍。
    if blobs:

       max_b = find_max(blobs)
       cx=max_b[5]
       cy=max_b[6]
       cw=max_b[2]
       ch=max_b[3]
       img.draw_rectangle(max_b.rect(),color=(0,0,0)) # rect
       img.draw_cross(max_b[5], max_b[6]) # cx, cy

调整好红色阈值后，赋值LAB阈值的参数，并赋值给red_threshold，调用MicroPython函数库中的image.find_blobs()函数，对该色域进行识别。

blob是色块的对象：

blob.rect() 返回这个色块的外框——矩形元组(x, y, w, h)，可以直接在image.draw_rectangle中使用。

1.blob.x() 返回色块的外框的x坐标（int），也可以通过blob[0]来获取。

2.blob.y() 返回色块的外框的y坐标（int），也可以通过blob[1]来获取。

3.blob.w() 返回色块的外框的宽度w（int），也可以通过blob[2]来获取。

4.blob.h() 返回色块的外框的高度h（int），也可以通过blob[3]来获取。

5.blob.pixels() 返回色块的像素数量（int），也可以通过blob[4]来获取。

6.blob.cx() 返回色块的外框的中心x坐标（int），也可以通过blob[5]来获取。

7.blob.cy() 返回色块的外框的中心y坐标（int），也可以通过blob[6]来获取。
8.blob.area() 返回色块的外框的面积。应该等于(w * h)

blobs列表里包含很多blob对象，blobs对象就是色块，每个blobs对象包含一个色块的信息。blobs就是很多色块。

三、STM32端

1.串口通信（接收）

usart3.c

int Openmv_X; /*OPENMV X Öá·´À¡×ø±ê*/
int Openmv_Y; /*OPENMV X Öá·´À¡×ø±ê*/
        
                           
void Uart3_Init(void)
{
	NVIC_InitTypeDef NVIC_InitStructure;
	GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;
	
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
	
	
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
	
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
	
	
	USART_InitStructure.USART_BaudRate = 9600;
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
	USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;
	USART_InitStructure.USART_Parity = USART_Parity_No;
	USART_InitStructure.USART_StopBits = USART_StopBits_1;
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;
	USART_Init(USART3, &USART_InitStructure);
	
	USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);
	
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
	
	
	NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
	NVIC_Init(&NVIC_InitStructure);
	
	USART_Cmd(USART3, ENABLE);
}
u8 flag;
void USART3_IRQHandler(void)			 
{
		u8 com_data; 
		u8 i;
		static u8 RxCounter1=0;
		static u16 RxBuffer1[5]={0};
		static u8 RxState = 0;	

		if( USART_GetITStatus(USART3,USART_IT_RXNE)!=RESET)  	   //½ÓÊÕÖÐ¶Ï  
		{
			 flag = 1;
				USART_ClearITPendingBit(USART3,USART_IT_RXNE);   //Çå³ýÖÐ¶Ï±êÖ¾
				com_data = USART_ReceiveData(USART3);
				if(RxState==0&&com_data==0x2C)  //0x2cÖ¡Í·
				{
					
					RxState=1;
					RxBuffer1[RxCounter1++]=com_data;
				}
		
				else if(RxState==1&&com_data==0x12)  //0x12Ö¡Í·
				{
					RxState=2;
					RxBuffer1[RxCounter1++]=com_data;
				}
		
				else if(RxState==2)
				{
					RxBuffer1[RxCounter1++]=com_data;

					if(RxCounter1==5 && com_data == 0x5B)       //RxBuffer1½ÓÊÜÂúÁË,½ÓÊÕÊý¾Ý½áÊø
					{
						
						Openmv_X=RxBuffer1[RxCounter1-3];//[2]
						Openmv_Y=RxBuffer1[RxCounter1-2];//[3]
					
						RxCounter1 = 0;
						RxState = 0;	
					}
					else if(RxCounter1 > 5)
					{
						RxState = 0;
						RxCounter1=0;
						for(i=0;i<5;i++)
						{
								RxBuffer1[i]=0x00;      //½«´æ·ÅÊý¾ÝÊý×éÇåÁã
						}
					
					}
				}
				else   //½ÓÊÕÒì³£
				{
						RxState = 0;
						RxCounter1=0;
						for(i=0;i<5;i++)
						{
								RxBuffer1[i]=0x00;      //½«´æ·ÅÊý¾ÝÊý×éÇåÁã
						}
				}

		}
		
}

注意：1.K210要与STM32共地。

2.K210要与STM32波特率一致，一般为9600。

2.舵机(定时器配置）

一个定时器有多个通道，不同通道可以设置不同的CCR寄存器的值来控制不同的舵机角度。

TIM3_PWM_Init(2000-1,720-1); 	//72M/2000/720=50Hz~20ms 
TIM_SetCompare1(TIM3,out_x);
TIM_SetCompare2(TIM3,out_y);

舵机角度与PWM参数关系的推导：

servo.c函数封装：

#define ARR 1999//TIM3 ARR值
#define MAX_Angle 270//270的舵机

float ccr1,ccr2;
void Servo1_SetAngle(float Angle)
{
	ccr1=ARR*Angle/10/MAX_Angle+ARR/40;
	TIM_SetCompare1(TIM3,ccr1);
}
void Servo2_SetAngle(float Angle)
{
	ccr2=ARR*Angle/10/MAX_Angle+ARR/40;
	TIM_SetCompare2(TIM3,ccr2);
}

由上面推导不难将PWM.c进一步封装可以直接在主函数中输入角度，免去了多次计算TIM_SetCompare1（）的参数。

注意：舵机供电电压要求5V，不要使用单片机供电（舵机电流较大），单片机与外接电源（给舵机供电）要共地

3.PID

TIM2_Int_Init(3000-1,840-1);	//30ms 定时器中断采样

void TIM2_IRQHandler(void)
{
	if(TIM_GetITStatus(TIM2,TIM_IT_Update)!=RESET)//ÅÐ¶ÏÖÐ¶Ï
	{	
		implement();
		TIM_ClearITPendingBit(TIM2,TIM_IT_Update);//ÊÖ¶¯Çå³ýÖÐ¶Ï±êÖ¾Î»
	}
}

void implement()
{
	int ccrx,ccry;

	if(flag)
	{
		flag = 0;
		
		ccrx = PID_Level(Openmv_X);
		ccry = PID_vertical(Openmv_Y);
		
		out_x = out_x - ccrx;
		out_y = out_y - ccry;

		
		TIM_SetCompare1(TIM3,out_x);
		TIM_SetCompare2(TIM3,out_y);
	}
}

TIM2每30ms根据串口接收到的坐标进行PID运算调整TIM3的CCR使舵机进行稳定追踪

pid.c

#include "pid.h"
PID pid;

void PID_Init()
{
	pid.X_Kp = 0.5;
	pid.X_Ki = 0;
	pid.X_Kd = 0.01;
	pid.X_err = 0;
	pid.X_err_sum = 0;
	pid.X_err_last = 0;
	
	pid.Y_Kp=0.5;
	pid.Y_Ki=0;
	pid.Y_Kd=0.01;
	pid.Y_err=0;
	pid.Y_err_sum=0;
	pid.Y_err_last=0;
}

//Ë®Æ½·½Ïò
int PID_Level(int x)
{
	int out;
	
	pid.X_err = x-50 ;
	pid.X_err_sum += pid.X_err;
	out = pid.X_Kp*(pid.X_err)+ pid.X_Ki*(pid.X_err_sum)+ pid.X_Kd*(pid.X_err - pid.X_err_last);
	pid.X_err_last = pid.X_err;
	
	return out;
}

//´¹Ö±·½Ïò
int PID_vertical(int y)
{
	int out;
		
	pid.Y_err = y-50 ;
	pid.Y_err_sum += pid.Y_err;
	out = pid.Y_Kp*(pid.Y_err)+ pid.Y_Ki*(pid.Y_err_sum)+ pid.Y_Kd*(pid.Y_err - pid.Y_err_last);
	pid.Y_err_last = pid.Y_err;
	
	return out;
}

pid.h

typedef struct 
{
	float X_Kp;
	float X_Ki;
	float X_Kd;
	float X_err;
	float X_err_sum;
	float X_err_last;	
	
	float Y_Kp;
	float Y_Ki;
	float Y_Kd;
	float Y_err;
	float Y_err_sum;
	float Y_err_last;	
}PID;

void PID_Init(void);
int PID_Level(int x);
int PID_vertical(int y);

Auroraisme

关注

6
点赞
踩
109

收藏

觉得还不错? 一键收藏
6
评论
基于STM32与K210的云台追踪装置

本文通过K210作为是识别模块去识别被测物体（以红色小球为例），当其识别到红色小球后，判断小球中心点所在的区域信息，并将其区域标志位通过串口发送给STM32，当STM32接收到位置信息后对x轴、y轴的两个舵机参数进行操作，最后通过定时器输出合适的PWM波，控制舵机旋转相应的角度，使K210摄像头对准被测物体，以实现物体追踪功能。
复制链接

扫一扫