简介
本文主要以如何实现pid调节部分作为主体,目标检测以及openmv与stm32之间的通信作为了解对象。本文将用最浅显易懂的例子将算法的核心展示给大家,以达到快速上手的目的。具体的问题描述可以详细参考2023年电赛控制类赛题,这里只做下简单描述。
一、需求的简单描述
项目需要我们使用舵机云台设计一个自动追踪绿色光点的功能,我们将使用舵机云台,云台顶部为视觉识别系统云台上有红色激光器恰恰处于相机的中心,OPENMV会将绿色光点与红色光点的坐标差传递给STM32。STM32以此来驱动云台以达到目标
二、stm32中pid算法代码
话不多说咱们先上代码,相信各位大佬不会存在理解的问题,如果是第一次尝试pid算法的同学请先看下一小段。
值得大家注意的是头文件并没有写的很精细,有一些无用代码可以根据情况删除。头文件中的#include "./systick/bsp_systick.h" 内涵系统滴答计时程序。这里用自己的进行替换即可(如果你的设备需要使用的话)#include "bsp_uart.h" 内涵openmv与stm32之间的串口通信程序。最终将为我们的程序输出目标点坐标与当前坐标的差值。并且本文中所提及到的多级是270度舵机安装方式如下图:
源文件如下
#include "./pid/pid.h"PID_POS pid_steer; //舵机云台追踪色块位置环PID结构体x轴PID_POS pid_steer_y; //舵机云台追踪色块位置环PID结构体y轴float PID_position(PID_POS *pid_pos,float actual_data){ pid_pos->EK =pid_pos->Sv - actual_data; // 计算当前坐标误差 pid_pos->SUM_EK+=pid_pos->EK; //误差累加 pid_pos->OUT = pid_pos->Kp * (pid_pos->EK) //比例P + pid_pos->Ki * pid_pos->SUM_EK //积分I + pid_pos->Kd * (pid_pos->EK - pid_pos->EK_NEXT); //微分D pid_pos->EK_NEXT = pid_pos->EK; // 更新上次误差 return pid_pos->OUT; }
上面这一短代码我们简单初始化了pid算法所需要的结构体,这个结构体可以在下面的头文件中查找到。并且完成pid算法最核心的部分。这里有一个十分值得注意的是,我们需要计算出x或y轴的距离即实际值与我们希望的目标值之间的差距。这个实际值是通过串口告诉我们的坐标差,目标值可以设置为0。因为我们希望的坐标差是0,坐标差为0就是我们调节的目标。这里便是与速度pid调节略微不同之处。返回值为最终输出结果。
void set_pid_pos_param(PID_POS *pid_pos,float p,float i,float d,float sv){ pid_pos->Kp=p; pid_pos->Ki=i; pid_pos->Kd=d; pid_pos->Sv=sv;}float abs_s(float a){ float temp; if(a>=0) { temp=a; } if(a<0) { temp=-a; } return temp;}
这里的set_pid_pos_param 函数为设置pid的初始参数,将在主函数中使用。abs_s 是一个简单的取正函数。
void steer_to_track(int angle)//传入的基准pwm{ static int x_er=0,y_er=0; //定义两个变量去接收OPENMV传送过来的坐标偏差值 static float pid_steer_out=0; //X轴PID输出量 static float pid_steer_out_y=0; //Y轴PID输出量 x_er = coordinate[0][0] ; y_er = coordinate[0][1] ;//这里将接收到的串口数据进行赋值 if(x_er>50000) { x_er = -(65535 - x_er); } if(y_er>50000) { y_er = -(65535 - y_er); }//注意这里的限制,主要是针对与小伙伴编写的视觉识别程序特定的。不是必须代码。需要根据你们的设备来确定需不需要添加限制。 pid_steer_out_y+=PID_position(&pid_steer_y,y_er); if(pid_steer_out_y>=1900) //舵机角度限幅,这个根据自己的设备确定。 pid_steer_out_y=1900; if(pid_steer_out_y<=-1900) pid_steer_out_y=-1900; pid_steer_out+=PID_position(&pid_steer,x_er); if(pid_steer_out>=1900) //舵机角度限幅,以防转多了卡住,这个根据自己的设备确定。 pid_steer_out=1900; if(pid_steer_out<-1900) pid_steer_out=-1900; //我这里使用的是tim8定时器,这里是直接控制翻转电平来达到舵机移动的目的。 TIM_SetCompare2(TIM8,(angle+pid_steer_out)); TIM_SetCompare1(TIM8,(angle+pid_steer_out_y));}
小伙伴们必须注意的是这里的基准pwm,因为云台安装需求,所以我们的云台舵机是有一定初始角度的,进而我们必须要在这个初始角度所对应的翻转电平上进行加减才能达到旋转云台的目的。这个基准pwm需要通过计算或者测试找出,也就是说需要看一下设备在初始化状态时的电平,这个电平就是我们需要输入的基准pwm。
对应头文件如下