简介

本文主要以如何实现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。

对应头文件如下

#include "./m/bsp_m.h"
#include "bsp_uart.h"
#include "./systick/bsp_systick.h" 
//bsp_m.h主要包含电机驱动代码
typedef struct 
{

    float Sv;
 float Kp;
 float Kd;
 float Ki;
 float EK;
 float EK_NEXT;
 float SUM_EK;
 float OUT;
}PID_POS;

float PID_position(PID_POS *pid_pos,float actual_data);
void set_pid_inc_param(PID_INC *pid_inc,float p,float i,float d,float sv);
void set_pid_pos_param(PID_POS *pid_pos,float p,float i,float d,float sv);
void steer_to_track(int angle);//传入的基准pwm
float abs_s(float a);
extern PID_POS pid_dis;            
extern PID_POS pid_encoder;     
extern PID_POS pid_steer;       
extern PID_POS pid_steer_y;     
extern int E_flag;
```

主文件如下

#include "stm32f10x.h"
#include "./usart/bsp_uart.h" 
#include "./systick/bsp_systick.h" 
#include "stdio.h"
#include "./pid/pid.h"
#include "./m/bsp_m.h"
#include
#include "./key/bsp_key.h" 


//初始化点，防止舵机堵转
void coordinate_init()
{
 for(int i = 0;i < 4;i++)
 {
  coordinate[i][0] = 0;
  coordinate[i][1] = 0;
 }
}
float k = 0.00008;
float i = 0.0;
float d = 0.0;
float x_set = 0;
float y_set = 0;

int main(void)
{ 
 int key_num; 
 SysTick_configuration();
 PWM_Out_Config_B();//初始化tim8以驱动电机
 USART_Config();
 coordinate_init();//初始化点，防止舵机堵转

 set_pid_pos_param(&pid_steer,k,i,d,x_set);      //舵机云台追踪色块位置式PID参数设置   
 set_pid_pos_param(&pid_steer_y,k,i,d,y_set);      //舵机云台追踪色块位置式PID参数设置    

 while(1)
 {
  steer_to_track(1500);
 }

}

这里的pid参数要根据自身情况进行调整，这里只是测试实例参数，无法直接使用。有经验的小伙伴就会发现，并没有给pid调节写专门的定时器，这是由于程序整体较为简单。如果有需求的小伙伴可以将steer_to_track(1500); 放置于任意定时器的中断即可。

三、一个pid的例子

简述

pid的示意图如下：

假设我们现在要对一个小车的速度进行pid调节。速度我们设为v，油门我们为pwm即p。假设p与v的比例为一比一。

我们先单独了解一下比例调节。比例调节的公式:比例(pi)*(目标速度-当前速度)

首先，我么假设比例为p=2目标速度设为100，假设在第一次检测的时候小车的实际速度为120，那么我们可以根据第一次检测到的速度数据进行比例计算。得到的速度就是我们目标的速度。第一轮计算2*(100-120)=-40。这里-40的意义便是小车下一次需要达到-40的速度，又由于速度与油门的比例是一比一，所以我们可以将速度的值设为油门值。显而易见的目标速度低于当前检测到的实际速度，小车这时应当减速。第二轮时我们不一定会在速度归零时获得检测速度，假设我们的检测点的速度是60。看到这里有的小伙伴就要发问了，为什么恰好就在60时检测数据，实际上我们在70，50，0等检测都是可以的，这个完全根据检测的间隔时长决定。我们再完成4轮计算如下：

我们进行了下面四组计算可以得到一些数据，我们将这些数据绘制成曲线如下

我们可以初步发下灰色曲线所显示的当前速度逐步向黄色的曲线靠拢。也就是说当前速度最终会稳定在一个平稳值上下波动。如果给位小伙伴继续向下计算就会发现，数据会大约在66.66上下进行波动。66.66就是最终平稳值，100是目标值。这两个值之间的差值便是稳态误差。单单使用比例调节时就会产生稳态误差，这时我们就会引入积分I。

积分调节。公式：I*(误差1+误差2+等等)

这个就是不断补偿的意思，我们假设这几次的误差为-20，40，25等等。那么比例调节的计算式便是：I*(-20+40+25)。通过积分去补偿这一稳态误差。但是这里又引出了一个新的问题：超调并且会发生震荡。

我们在这里进行一个新的假设：检测到的数据分别为96，98，99。

从上面这个草图中我们可以看到积分计算的结果在逐渐变大但是曲线却在接近目标值。因此我们得出一个结论当我们的曲线越接近目标值时震动会变得更大，而不是边小，因此最终会在目标速度上下波动。这里就需要我们引入微分调节，加入阻尼，以改变超调的现象。

比例调节。公式:d*(本次误差-上次误差)

假设依然按照上一个例子来看，第一次：D(2-4)=-2D，第二次：D(1-2)=-D。我们可以发现阻尼越接近目标值阅大抵消了积分调节所产生的超调显现。将上述三个算式加起来就是最终pid的算式。注意这是一轮下来pid的计算结果。pid要不断地循环。