【OpenMV+STM32】PID控制二维自由舵机色块追踪

这篇具有很好参考价值的文章主要介绍了【OpenMV+STM32】PID控制二维自由舵机色块追踪。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

一、所用工具

1、芯片:STM32F407ZGT6

2、CubeMX

3、KEIL5

4、OpenMV 

5、舵机

二、实现功能

        利用由两个自由舵机组装而成的二维云台来控制OpenMV的位置,以实现追踪指定阈值色块的效果。

三、CubeMX配置

3.1 初始化配置

【OpenMV+STM32】PID控制二维自由舵机色块追踪,stm32,嵌入式硬件,单片机【OpenMV+STM32】PID控制二维自由舵机色块追踪,stm32,嵌入式硬件,单片机

【OpenMV+STM32】PID控制二维自由舵机色块追踪,stm32,嵌入式硬件,单片机

3.2 定时器配置(PWM波输出)

        这里我使用TIM3的通道1和TIM4的通道1分别实现对两个舵机的控制,由于我购买的舵机所需要的频率为50Hz(如下图)所以下面的PWM频率均是按照50Hz配置的。

【OpenMV+STM32】PID控制二维自由舵机色块追踪,stm32,嵌入式硬件,单片机

【OpenMV+STM32】PID控制二维自由舵机色块追踪,stm32,嵌入式硬件,单片机

        TIM4的配置同理。

【OpenMV+STM32】PID控制二维自由舵机色块追踪,stm32,嵌入式硬件,单片机

3.3 串口通信配置(与OpenMV和电脑通信)

        为了方便后续的调试,所以建议同时开启与OpenMV和电脑的通信,即打开两个串口。USART1是用来和电脑通信的,USART2是用来与OpenMV通信的。

【OpenMV+STM32】PID控制二维自由舵机色块追踪,stm32,嵌入式硬件,单片机

        USART2的配置同理。

【OpenMV+STM32】PID控制二维自由舵机色块追踪,stm32,嵌入式硬件,单片机

四、OpenMV配置

        这是OpenMV中寻找色块,并通过串口PA4,PA5发送中心点位置以及长度和宽度的代码,可以直接复制到OpenMV IDE中。需要注意的是,在OpenMV中UART_RX—P5 ------ UART_TX—P4。

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 = 0                             # 0 for red, 1 for green, 2 for blue
thresholds = [
    (30, 100, 15, 127, 15, 127),  # generic_red_thresholds
    (30, 100, -64, -8, -32, 32),  # generic_green_thresholds
    (0, 30, 0, 64, -128, 0),      # generic_blue_thresholds
    (82, 100, 75, -49, -22, 31),  # generic_white_thresholds
    (21, 83, 32, 65, 31, 63),
]

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,w,h):
    global uart;
    FH = bytearray([0xb3,0xb3])     # 帧头
    uart.write(FH)                  # 写到串口

    uart.write(str(x))
    uart.write(bytearray([0x20]))   # 发送空格
    uart.write(str(y))
    uart.write(bytearray([0x20]))
    uart.write(str(w))
    uart.write(bytearray([0x20]))
    uart.write(str(h))

    uart.write(bytearray([0x20]))
    FH = bytearray([0x0d,0x0a])     # 帧尾,换行和回车的ascll
    uart.write(FH)



while True:
    clock.tick()
    img = sensor.snapshot()
    blobs = img.find_blobs([thresholds[threshold_index]])

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

        # 这些值取决于max_blob不是圆形的,否则它们将不稳定.
        # 检查max_blob是否显著偏离圆形
        if max_blob.elongation() > 0.5:
            img.draw_edges(max_blob.min_corners(), color=(255, 0, 0))
            img.draw_line(max_blob.major_axis_line(), color=(0, 255, 0))
            img.draw_line(max_blob.minor_axis_line(), color=(0, 0, 255))

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

        # 注意-max_blob旋转仅限于0-180。
        img.draw_keypoints(
            [(cx, cy, int(math.degrees(max_blob.rotation())))], size=20
        )

        send_data(cx,cy,cw,ch)      # 发送数据

        print(cx,cy,cw,ch)
        print(clock.fps())
   

五、KEIL代码修改

5.1 串口重定义

        在usart.c的最后加入重定义代码。

/* USER CODE BEGIN 1 */
int fputc(int ch, FILE *f)
{
  HAL_UART_Transmit(&huart1, (uint8_t *)&ch, 1, 0xffff);
  return ch;
}
int fgetc(FILE *f)
{
  uint8_t ch = 0;
  HAL_UART_Receive(&huart1, &ch, 1, 0xffff);
  return ch;
}

/* USER CODE END 1 */

         在usart.h的开头加入include。

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

5.2 串口回调函数

       用到的变量初始化,大家自行放到main.c中的相应位置。


#include "string.h"
#include "stdio.h"
#include "stdlib.h"
 
#define RXBUFFERSIZE  256
 
char RxBuffer[RXBUFFERSIZE],rx_buf[RXBUFFERSIZE];
uint8_t aRxBuffer;
uint8_t Uart1_Rx_Cnt = 0;
int flag=0;
 
printf("Hello World!\r\n");
HAL_Delay(200);
HAL_UART_Receive_IT(&huart2, (uint8_t *)&aRxBuffer, 1);

        这段代码放在main.c中,这里为了方便确定OpenMV与STM32之间有数据传输,加入了翻转LED灯,大家可以有需要可以自己再配置一下LED,不用的话可自行删除。

/* USER CODE BEGIN 4 */
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    UNUSED(huart);
	if(huart==&huart2){
		HAL_GPIO_TogglePin(GPIOF,LED_Pin);		//有数据则翻转LED灯
 
		RxBuffer[Uart1_Rx_Cnt] = aRxBuffer;
		Uart1_Rx_Cnt++;
		if((RxBuffer[Uart1_Rx_Cnt-1] == 0xb3)&&(RxBuffer[Uart1_Rx_Cnt-2] == 0xb3)) flag=1;		 //帧头判定
		else if((RxBuffer[Uart1_Rx_Cnt-2] == 0x0d)&&(RxBuffer[Uart1_Rx_Cnt-1] == 0x0a)) flag=2;  //帧尾判定
		else flag=0;
 
		switch (flag)
        {
        	case 1:
				Uart1_Rx_Cnt = 0;
				memset(RxBuffer,0x00,sizeof(RxBuffer));
        		break;
        	case 2:
				RxBuffer[Uart1_Rx_Cnt-1] = '\0';
				RxBuffer[Uart1_Rx_Cnt-2] = '\0';
				strcpy(rx_buf,RxBuffer);
				printf("%s\r\n",rx_buf);
				while(HAL_UART_GetState(&huart2) == HAL_UART_STATE_BUSY_TX);
				Uart1_Rx_Cnt = 0;
				memset(RxBuffer,0x00,sizeof(RxBuffer));
        		break;
			default:break;
        }
	HAL_UART_Receive_IT(&huart2, (uint8_t *)&aRxBuffer, 1);
	}
}
/* USER CODE END 4 */

5.3 加入pid控制文件

5.3.1 pid.c

        由于两个舵机分别控制横向和纵向的移动,所以此处定义了两个句柄和两个pid函数,分别对应两个舵机,大家可以自行调试其中Kp,Ki,Kd的值。

#include "pid.h"

pid_typedef pid1;
pid_typedef pid2;

void PID_init(void)

{
   
    pid1.SetPosition=0;
    pid1.ActualPosition=0.0;
    pid1.err=0.0;
    pid1.err_last=0.0;
    pid1.out=0.0;
    pid1.integral=0.0;
    pid1.Kp=0.025;
    pid1.Ki=0;
    pid1.Kd=0.017;
  
	pid2.SetPosition=0;
    pid2.ActualPosition=0.0;
    pid2.err=0.0;
    pid2.err_last=0.0;
    pid2.out=0.0;
    pid2.integral=0.0;
    pid2.Kp=0.025;
    pid2.Ki=0;
    pid2.Kd=0.017;
}



float PIDx_realize(float ActualPosition,float SetPosition)
{
	pid1.ActualPosition=ActualPosition;
	pid1.SetPosition=SetPosition;
    pid1.err=pid1.SetPosition-pid1.ActualPosition;
    pid1.integral+=pid1.err;
    pid1.out=pid1.Kp*pid1.err+pid1.Ki*pid1.integral+pid1.Kd*(pid1.err-pid1.err_last);
    pid1.err_last=pid1.err;

    return pid1.out;
}

float PIDy_realize(float ActualPosition,float SetPosition)
{
	pid2.ActualPosition=ActualPosition;
	pid2.SetPosition=SetPosition;
    pid2.err=pid2.ActualPosition-pid2.SetPosition;
    pid2.integral+=pid2.err;
    pid2.out=pid2.Kp*pid2.err+pid2.Ki*pid2.integral+pid2.Kd*(pid2.err-pid2.err_last);
    pid2.err_last=pid2.err;

    return pid2.out;
}
 5.3.2 pid.h
#ifndef __PID_H
#define __PID_H
#include "stm32f4xx.h"

typedef struct
{
	float SetPosition;//设定值
	float ActualPosition;//实际值
	float err;
	float err_last;
	float Kp;
	float Ki;
	float Kd;
	float out;//执行器的变量
	float integral;//积分值
}pid_typedef;

void PID_init(void);
float PIDx_realize(float ActualPosition,float SetPosition);
float PIDy_realize(float ActualPosition,float SetPosition);

#endif

        添加好文件之后不要忘记添加pid.h的目录 。

5.4 实现对PWM波占空比的控制

        放置在前面的初始化代码。

#include "pid.h"

double motor1=25;
double motor2=35;
int cx,cy;

PID_init();
HAL_Delay(100) ;
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);//开启PWM波
HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_1);
__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, motor1);//占空比初始化,为25\1000=2.5%
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, motor2);

        放在中断回调函数中的代码,可以选择放在case2中的printf下面,因为在OpenMV中设定的QVGA的中心坐标为160,120,所以下列的设定横纵坐标分别为160和120。其中舵机2我设定的占空比在3.5%和6%之间是因为我的装置结构问题,如果上面的电机往下太多会导致数据线与底座接触,造成电机卡住,大家可以根据自己的装配情况自行更改占空比的上下范围。

sscanf(rx_buf, "%d %d", &cx, &cy);//提取rx_buf中的前两个数字,即为横纵坐标
//对下面对应横坐标的舵机进行控制
motor1=motor1+PIDx_realize(cx,160);//第一参数为实际坐标,第二个参数为设定坐标
if(motor1<25)motor1=25;//防止超过舵机的工作范围内的占空比
if(motor1>125)motor1=125;
TIM3->CCR1=motor1;
//对上面对应纵坐标的舵机进行控制
motor2=motor2+PIDy_realize(cy,120);
if(motor2<35)motor2=35;
if(motor2>60)motor2=60;
TIM4->CCR1=motor2;

六、成果展示

        由于实验室器材有限,无奈拿了一个开学典礼的灯作为底座,所以在舵机运行起来时会不稳,建议大家把舵机固定在重一点且高一点的东西上,可以避免OpenMV的线被卡住。总体来说,摄像头的跟踪效果还是不错的。

色块跟踪

七、结语

        大家如果想要购买我的同款舵机,可以私信我,但是这个舵机的装配比较麻烦,需要自己用工具把材料修剪到合适的大小。然后这是我写的第一份博客,内容可能有不足的地方,大家都可以指出,最后为大家分享几篇我在做的过程中参考的文章,其中第一篇来自我同实验室的同学。

【STM32+HAL】与OpenMV通信

STM32与openmv通信(HAL库)

位置式PID与增量式PID区别浅析文章来源地址https://www.toymoban.com/news/detail-841349.html

到了这里,关于【OpenMV+STM32】PID控制二维自由舵机色块追踪的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • STM32——舵机控制

    一、舵机控制原理 (一)概述         舵机是一种位置伺服驱动器器,是一种带有输出轴的小装置。当我们向伺服器发送一个控制信号时,输出轴就可以转到特定的位置。只在控制信号持续不变,伺服机构就会保持相对的角度位置不变。如果控制信号发生变化,输出轴的位

    2024年04月23日
    浏览(41)
  • STM32控制舵机精准角度

    一、舵机原理 二、程序介绍 三、精准角度控制原理 四、程序分享         网上很多教程,都只是控制舵机0°,45°,90°等特定的角度,比如1°,很多程序都做不到,即使有也少得可怜,更不会详解介绍是怎么实现精准控制每一度的,因此,这篇文章的意义就在这里。 一

    2024年02月15日
    浏览(38)
  • stm32同时控制两个舵机

    硬件准备:stm32,两个舵机   舵机pwm波形输出,使用TIM2中的两个通道进行输出。 pwm.c pwm,h 为了更加直观的使用,用Servo函数对pwm输出占空比与舵机旋转角度进行封装。 Servo,c Servo,h 主函数 同时分享一位大佬的视频,我也是跟着他学习,一步一步总结的。 https://www.bilibili.com/vi

    2024年02月13日
    浏览(35)
  • OpenMV输出PWM,实现对舵机控制

    OpenMV的定时器官方函数介绍:Timer类 – 控制内部定时器 目录 OpenMV的PWM资源介绍 为什么要用OpenMV输出PWM OpenMV的PWM资源分配 资源 注意  建议 PWM输出代码 代码讲解 Timer Timer.channel tim.channel与Timer.channel区别 Timer.channel解析 OpenMV有9个外接IO可以供我们使用, 很多时候我们只是把

    2024年02月03日
    浏览(31)
  • 基于STM32 SG90 9g舵机控制

    产品参数 名称:9克舵机180度 尺寸:23mm X 12.2mm X 29mm 重量:9克 扭矩:1.5kg/cm 工作电压:4.2 - 6V 温度范围:0 - 55℃ 运行速度:0.3s/60℃ 死带宽:10ms 舵机有三条线定义: 暗灰:GND 红色:VCC 4.8V - 7.2V 橙黄线:脉冲输入 在机器人机电控制系统中,舵机控制效果是性能的重要影响

    2024年02月14日
    浏览(44)
  • STM32输出PWM波控制180°舵机

    时间记录:2024/2/8 (1)脉冲宽度调制 (2)占空比:高电平时间占整个周期时间的比例 (3)STM32通过定时器实现PWM时具有两种模式 PWM1模式:向上计数模式下,CNT当前计数寄存器的值小于CCR比较寄存器的值时输出有效电平,大于时输出无效电平,向下计数模式下,CNT当前计数

    2024年02月21日
    浏览(47)
  • 基于STM32F1的舵机控制

    一、SG90舵机介绍 SG90是有三个版本的,90度版、180度版和360度版,前两个只是舵机旋转角度的范围不一样,可以在这个范围内任意的控制舵机所转的角度,而360版本的是一直旋转的,我们不能控制它旋转的角度,只能控制它旋转的速度,这里我们在买的时候就要注意一点,根

    2023年04月25日
    浏览(278)
  • stm32中如何使用PCA9685控制舵机(详解)

    最近在公司实习,都说跨入职场是让人进步最快的方式,记录一下我自学加同学指点学到新知识。 PCA9685是一个基于IIC通信的16路PWM输出模块,可以在单片机资源不足的情况下进行扩展使用。 i2c通信,只需要2根i2c线就可以控制16路pwm,周期和占空比都可控。 可以多个模块级联

    2024年02月06日
    浏览(51)
  • STM32第七课:PWM控制SG90舵机

            学习完上一课的PWM控制LED小灯实现呼吸灯的效果,我们就可以进一步学习PWM控制舵机的效果了。PWM控制舵机相信会是一个更有意思的小实验的。          舵机是一种位置(角度)伺服的驱动器,适用于那些需要角度不断变化并可以保持的控制系统。目前在高档

    2024年04月17日
    浏览(32)
  • STM32---SG90舵机控制(HAL库,含源码)

    写在前面: 在嵌入式的项目中,舵机是一个十分常见的元器件模块,其主要的功能是实现机械转动,实质上舵机是一个伺服的驱动器,适用于那些需要角度不断变化并可以保持的控制系统。例如在机器人的电控制器系统中,飞行器设计,遥控机器人等。舵机的使用需要PWM信号

    2024年04月13日
    浏览(32)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包