STM32 + OPENMV 实现二维云台追踪小球

原创已于 2025-02-14 16:04:47 修改 · 876 阅读

5 ·

CC 4.0 BY-SA版权

文章标签：

#stm32 #嵌入式硬件 #单片机

于 2025-02-03 23:47:40 首次发布

视频链接：Openmv+stm32 云台视觉追踪_哔哩哔哩_bilibili

二维云台是淘宝上买的，用的是数字舵机，精度还可以，主控为STM32F103C8T6，单片机与openmv之间使用串口通信，通信格式采用json字符串。

程序逻辑很简单，openmv识别色块，持续发送小球的相对于以画面中心为原点的坐标系的坐标，由stm32进行pid控制，使小球保持在画面中心。

由于程序非常简单，所以pid直接放在空闲中断里进行。

舵机部分:

#include "servo.h"
#include "tim.h"

double roll_deg = 0,pitch_deg = 0;

void roll_angle(double deg)	//-135~135 -- 500~2500
{
	double temp = ((deg-(-135.0))/270.0)*2000 + 500;
	__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_1,temp);
}

void pitch_angle(double deg)	//-90~90 -- 500~2500
{
	double temp = ((deg-(-90.0))/180.0)*2000 + 500;
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,temp);
}

PID部分：

#include "PID.h"
#include "usart.h"
#include "servo.h"

extern double roll_deg,pitch_deg;
extern int pos_x,pos_y;

double p_roll = 0.03;
double d_roll = 0.11;

double p_pitch = 0.03;
double d_pitch = 0.07;

double error_roll[2]; //0-->before,1-->current
double error_pitch[2];

void pid_calc(void)
{
	error_roll[1] = pos_x;
	error_pitch[1] = pos_y;
	
	roll_deg = roll_deg - (error_roll[1]*p_roll + (error_roll[1] - error_roll[0])*d_roll);
	pitch_deg = pitch_deg - (error_pitch[1]*p_pitch + (error_pitch[1] - error_pitch[0])*d_pitch);
	
	Limit(&roll_deg, 135, -135);
	Limit(&pitch_deg, 90, -90);
	if(pos_x == 1000 && pos_y == 1000)
	{
		roll_deg = 0;
		pitch_deg = 0;
	}
	
	roll_angle(roll_deg);
	pitch_angle(pitch_deg);
	
	error_roll[0] = error_roll[1];
	error_pitch[0] = error_pitch[1];
}

void Limit(double *val, double max, double min)
{
	if(*val < min) *val = min;
	if(*val > max) *val = max;
}

主程序（空闲中断）：

void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
{
	if(huart->Instance == USART1)
	{
		Jansson1_Analysis((char*)data);
		memset(data, 0, 100);
		USART2_Printf("x:%d,y:%d\n",pos_x,pos_y);
		
		pid_calc();
	}
}

Openmv部分：

import sensor
from machine import UART
import json
import display
import image

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)     #320x240
sensor.skip_frames()
sensor.set_auto_whitebal(False)
sensor.set_hmirror(True)
sensor.set_vflip(True)
sensor.set_transpose(False)

uart = UART(1,115200)
pos_x = 0
pos_y = 0
green_thresholds = (0, 96, -42, -9, -8, 37)
cnt = 0

def find_Max(blobs):       
    max_size = 0
    for blob in blobs:
        if blob.w()*blob.h() > max_size:
            max_blob = blob
            max_size = blob.w()*blob.h()
            return max_blob

lcd = display.SPIDisplay()

while True:

    img = sensor.snapshot()

    blobs = img.find_blobs([green_thresholds],pixels_threshold=3000)
    if blobs: #必须加if，如果find_blobs返回none则会出错
        blob = find_Max(blobs)
        pos_x = blob.cx() - 160    //转换为画面中心为原点的坐标
        pos_y = 120 - blob.cy()
        img.draw_rectangle(blob.rect())
    else:
        cnt += 1
        if cnt > 10:
            pos_x = 0
            pos_y = 0
            cnt = 0
    lcd.write(img,hint=image.ROTATE_180)
    str = {"pos_x":pos_x,"pos_y":pos_y}
    uart.write(json.dumps(str))