以OpenMV和STM32搭建的二维舵机云台——浅谈增量式PID算法
这是最近接到了一单设计需要做一个追物块的二位云台，联想到之前直接用OpenMV做的PID云台，想打算直接用OpenMV做一个二维云台了事，结果客户需要利用STM32去设计云台，像RMB低头，只能做！这才有如今这篇文章。
该设计是通过OpenMV识别红色物块坐标，直接将色块的二维坐标通过OpenMV的串口传入到STM32F103C8T6中，所有过程均通过STM32F103C8T6进行处理。一开始是直接打算利用增量式PID去求得二维舵机的角度的。结果没有调通，在此期间也问了一些博主，后来他也不回我了，无奈之下，我只能尝试用P比例算法一点一点的调整。但是过程实在是太慢，于是再交货的今天，我决定再次攻占增量式PID算法。
前言说明
元器件清单说明
除此种种，还要再加上OpenMV一个。我采用的是OpenMV Cam H7.
原理图绘制
硬件平台介绍
PCA9685模块
舵机控制板PCA9685与STM32F03C8T6的I2C接口相连。PCA9685的SDA、SCL引脚分别接STM32的PB6、PB7引脚。舵机控制板 PCA9685是一款可以输出高达16路PWM的芯片，只需将舵机接到各路引脚上，通过输出不同的PWM波形就可以调节各个舵机的角度。它内置25MHz晶振，每一路有12位分辨率。该二位云台采用的都是20ms为时基的舵机，高电平的范围是0.5ms到2ms之间，因此舵机的驱动信号是50Hz的方波信号。
具体公式我就不一一说明了，102是0所对应的占空比。
TD8120舵机介绍
采用的数字舵机的控制都是以20ms左右为时基脉冲，该脉冲的高电平部分一般为0.5ms-2.5ms范围内的角度控制脉冲部分。脉冲为高电平与对应舵机的角度关系如表所示。
OLED等
不一一介绍了，简单。
增量式PID算法
该算法的原理是，为了使得OpenMV识别的物块在中间位置，而OpenMV的像素点是320*240的，于是中心位置是（160，120）
首先得到当前的误差值。
/*本次偏差值*/
pid.Error_X_Current = pid.Current_X_Location - pid.Set_X_Location; //本次偏差值
/*本次偏差值*/
pid.Error_Y_Current = pid.Current_Y_Location - pid.Set_Y_Location; //本次偏差值
如何设置PID算法的终止条件呢？进行判断即可啦！废话不说，附代码。
void Data_Process(char *target,u8 *source,int n)
{
int count;
for(count=0;count<n;count++)
{
*target++=*source++;
}
}
void Receive_Process()
{
u16 i;
u16 Separator;
if(USART_RX_STA&0x8000)
{
if((USART_RX_BUF[0]==0XAA)&&(USART_RX_BUF[1]==0XAA))
{
Len_Buf = strlen((char*)USART_RX_BUF);
for(i=2;i<Len_Buf;i++)
{
if(USART_RX_BUF[i] == ',')
{
Separator=i;
}
}
Data_Process(X,USART_RX_BUF+2,Separator-2);
Data_Process(Y,USART_RX_BUF+Separator+1,Len_Buf-Separator);
pid.Current_X_Location=atoi(X);
pid.Current_Y_Location=atoi(Y);
if(abs(pid.Current_X_Location-160)<5&&abs(pid.Current_Y_Location-120)<5)
{
LED0=!LED0;
pca_setpwm(0,0,pid.Last_X_PWM);
X_Angle = 0.439 * (pid.Last_X_PWM-102);
sprintf(X_Angle_arr,"%4.2f",X_Angle);
OLED_ShowString(64,0,(uint8_t *)X_Angle_arr,16);
pca_setpwm(1,0,pid.Last_Y_PWM);
Y_Angle = 0.439 * (pid.Last_Y_PWM-102);
sprintf(Y_Angle_arr,"%4.2f",Y_Angle);
OLED_ShowString(64,4,(uint8_t *)Y_Angle_arr,16);
}
else
{
Pid_Calcuate();
}
// printf("%f\r\n",pid.Current_X_Location);
// printf("%f\r\n",pid.Current_Y_Location);
memset(USART_RX_BUF,0,sizeof(USART_RX_BUF));
memset(X,0,sizeof(X));
memset(Y,0,sizeof(Y));
USART_RX_STA=0;
}
else
{
memset(USART_RX_BUF,0,sizeof(USART_RX_BUF));
USART_RX_STA=0;
}
}
}
这一段代码主要是进行OpenMV发送的数据进行处理，待会附OpenMV的代码。处理得到两个坐标，进行判断。如果两个坐标与中心位置误差不超过5，则不进行PID计算，直接将上一次的PWM输出即可。自行理解咯！
if(abs(pid.Current_X_Location-160)<5&&abs(pid.Current_Y_Location-120)<5)
{
LED0=!LED0;
pca_setpwm(0,0,pid.Last_X_PWM);
X_Angle = 0.439 * (pid.Last_X_PWM-102);
sprintf(X_Angle_arr,"%4.2f",X_Angle);
OLED_ShowString(64,0,(uint8_t *)X_Angle_arr,16);
pca_setpwm(1,0,pid.Last_Y_PWM);
Y_Angle = 0.439 * (pid.Last_Y_PWM-102);
sprintf(Y_Angle_arr,"%4.2f",Y_Angle);
OLED_ShowString(64,4,(uint8_t *)Y_Angle_arr,16);
}
else
{
Pid_Calcuate();
}
增量式PID要来啦！由于我在搭载舵机云台的时候，我是可以把两个舵机置于90°进行搭载的，具体如图所示。因此是知道初始位置的，将这个值赋给他们
pid.Output_X_Current = 307;
pid.Output_Y_Current = 307;
这一步是在PID初始化中设置的，我待会儿直接上代码。
所以程序中我就要注意增量式赋值问题了。
先看增量式的原理，大家似乎有没有觉得奇怪，这个第二项怎么那么想位置式的P比例调节？一定要注意这个现象，在进行PID增量式调节的时候，需要先调节I参数，（位置式中是先Kp调节）。也就是我程序中Ki，图片上的KpT/T1。这里Ki的调整，得一点一点的往上加，由于一开始的位置中间位置，所以他会在一定范围内波动一下，然后恢复到正常的状态。我是从0.01往上加的，发现到0.07比较合适，为了进行kp和kd参数的调整，需要将ki的参数设置为比合适0.07略低一些，这里设置为0.06。然后对Kp的参数进行调整，这里是从大到小，大不能超过Ki的参数，最终设置为0.01，而kd只需kp的一半即可，也可以不用，这里设置0.005。参数介绍完了，上代码。
/***************************************
1.该函数时位置式PID算法，参数未进行调整
2.下面的注释代码是P比例调节和增量式PID算法
3.我测试过增量式PID算法，发现调节的P值不是很稳定，
于是转而用纯P比例算法
***************************************/
void Pid_Calcuate()
{
/*本次偏差值*/
pid.Error_X_Current = pid.Current_X_Location - pid.Set_X_Location; //本次偏差值
{
/*比例算法*/
pid.Proportion_X_Out = pid.Kp * (pid.Error_X_Current - pid.Error_X_Last); //初始状态pid.Kp*pid.Error_X_Current
/*积分算法*/
pid.Integral_X_Out = pid.Ki*pid.Error_X_Current;
/*微分算法*/
pid.Derivative_X_Out = pid.Kd * (pid.Error_X_Current - 2 * (pid.Error_X_Current - pid.Error_X_Last) + pid.Error_X_Last_Last);
/*PID算法输出*/
pid.Output_X = pid.Proportion_X_Out + pid.Integral_X_Out + pid.Derivative_X_Out;
pid.Output_X_Current += pid.Output_X;
if(pid.Output_X_Current>512)
{
pid.Output_X_PWM = 512;
}
else if(pid.Output_X_Current<102)
{
pid.Output_X_PWM = 102;
}
else
{
pid.Output_X_PWM = (int)pid.Output_X_Current;
}
pca_setpwm(0,0,pid.Output_X_PWM);
X_Angle = 0.439 * (pid.Output_X_PWM-102);
sprintf(X_Angle_arr,"%4.2f",X_Angle);
OLED_ShowString(64,0,(uint8_t *)X_Angle_arr,16);
pid.Error_X_Last_Last = pid.Error_X_Last;
pid.Error_X_Last = pid.Error_X_Current;
pid.Last_X_PWM = pid.Output_X_PWM;
}
/*本次偏差值*/
pid.Error_Y_Current = pid.Current_Y_Location - pid.Set_Y_Location; //本次偏差值
{
/*比例算法*/
pid.Proportion_Y_Out = pid.Kp * (pid.Error_Y_Current - pid.Error_Y_Last);
/*积分算法*/
pid.Integral_Y_Out = pid.Ki * pid.Error_Y_Current;
/*微分算法*/
pid.Derivative_Y_Out = pid.Kd * (pid.Error_Y_Current - 2 * (pid.Error_Y_Current - pid.Error_Y_Last) + pid.Error_Y_Last_Last);
/*PID算法输出*/
pid.Output_Y = pid.Proportion_Y_Out + pid.Integral_Y_Out + pid.Derivative_Y_Out;
pid.Output_Y_Current -= pid.Output_Y;
if(pid.Output_Y_Current>512)
{
pid.Output_Y_PWM = 512;
}
else if(pid.Output_Y_Current<102)
{
pid.Output_Y_PWM = 102;
}
else
{
pid.Output_Y_PWM = (int)pid.Output_Y_Current;
}
pca_setpwm(1,0,pid.Output_Y_PWM);
Y_Angle = 0.439 * (pid.Output_Y_PWM-102);
sprintf(Y_Angle_arr,"%4.2f",Y_Angle);
OLED_ShowString(64,4,(uint8_t *)Y_Angle_arr,16);
pid.Error_Y_Last_Last = pid.Error_Y_Last;
pid.Error_Y_Last = pid.Error_Y_Current;
pid.Last_Y_PWM = pid.Output_Y_PWM;
}
}
切记
pid.Output_X_Current = 307;
pid.Output_Y_Current = 307;
这里的初始值307是根据pca9685来进行设置的，反正是两个舵机的90°位置。
最后附上我完工的视频
追物块的云台——STM32+PID
，完整代码等那位给我RMB的兄弟写完论文开源附上，如有任何疑问，联系我（QQ：1679960378）
PCA9685代码
#include "pca9685.h"
#include "myiic.h"
#include "delay.h"
#include "math.h"
void pca_write(u8 adrr,u8 data)//向PCA写数据,adrrd地址,data数据
{
IIC_Start();
IIC_Send_Byte(pca_adrr);
IIC_Wait_Ack();
IIC_Send_Byte(adrr);
IIC_Wait_Ack();
IIC_Send_Byte(data);
IIC_Wait_Ack();
IIC_Stop();
}
u8 pca_read(u8 adrr)//从PCA读数据
{
u8 data;
IIC_Start();
IIC_Send_Byte(pca_adrr);
IIC_Wait_Ack();
IIC_Send_Byte(adrr);
IIC_Wait_Ack();
IIC_Start();
IIC_Send_Byte(pca_adrr|0x01);
IIC_Wait_Ack();
data=IIC_Read_Byte(0);
IIC_Stop();
return data;
}
void pca_setfreq(float freq)//设置PWM频率
{
u8 prescale,oldmode,newmode;
double prescaleval;
freq *= 0.92;
prescaleval = 25000000;
prescaleval /= 4096;
prescaleval /= freq;
prescaleval -= 1;
prescale =floor(prescaleval + 0.5f);
oldmode = pca_read(pca_mode1);//pca_mode1 0x0
newmode = (oldmode&0x7F) | 0x10; // sleep 0x0&0x7f=0x0 0x0|0x10=0x10
pca_write(pca_mode1, newmode); // go to sleep D4sleep写1
pca_write(pca_pre, prescale); // set the prescaler
//pca_pre 0xFE控制周期的寄存器，将计算过的预装载值放入
pca_write(pca_mode1, oldmode);//oldmode 0x0,D4写0退出sleep模式
delay_ms(2);
pca_write(pca_mode1, oldmode | 0xa1);
//0x0|0xa1=10100001 D6使用内部时钟，D5内部地址读写后自动增加，D0响应0x70通用i2c地址
}
void pca_setpwm(u8 num, u32 on, u32 off)
{
pca_write(LED0_ON_L+4*num,on);//
pca_write(LED0_ON_H+4*num,on>>8);
pca_write(LED0_OFF_L+4*num,off);
pca_write(LED0_OFF_H+4*num,off>>8);
}
/*num:舵机PWM输出引脚0~15，on:PWM上升计数值0~4096,off:PWM下降计数值0~4096
一个PWM周期分成4096份，由0开始+1计数，计到on时跳变为高电平，继续计数到off时
跳变为低电平，直到计满4096重新开始。所以当on不等于0时可作延时,当on等于0时，
off/4096的值就是PWM的占空比。*/
/*
函数作用：初始化舵机驱动板
参数：1.PWM频率
2.初始化舵机角度
*/
void PCA_MG9XX_Init(float hz,u8 angle)
{
u32 off=0;
IIC_Init();
pca_write(pca_mode1,0x0);
pca_setfreq(hz);//设置PWM频率
off=(u32)(145+angle*2.4);
pca_setpwm(0,0,off);pca_setpwm(1,0,off);pca_setpwm(2,0,off);pca_setpwm(3,0,off);
pca_setpwm(4,0,off);pca_setpwm(5,0,off);pca_setpwm(6,0,off);pca_setpwm(7,0,off);
pca_setpwm(8,0,off);pca_setpwm(9,0,off);pca_setpwm(10,0,off);pca_setpwm(11,0,off);
pca_setpwm(12,0,off);pca_setpwm(13,0,off);pca_setpwm(14,0,off);pca_setpwm(15,0,off);
delay_ms(500);
}
/*
函数作用：控制舵机转动；
参数：1.输出端口，可选0~15；
2.起始角度，可选0~180；
3.结束角度，可选0~180；
4.模式选择，0 表示函数内无延时，调用时需要在函数后另外加延时函数，且不可调速，第五个参数可填任意值；
1 表示函数内有延时，调用时不需要在函数后另外加延时函数，且不可调速，第五个参数可填任意值；
2 表示速度可调，第五个参数表示速度值；
5.速度，可填大于 0 的任意值，填 1 时速度最快，数值越大，速度越小；
注意事项：模式 0和1 的速度比模式 2 的最大速度大；
*/
void PCA_MG9XX(u8 num,u8 start_angle,u8 end_angle,u8 mode,u8 speed)
{
u8 i;
u32 off=0;
switch(mode)
{
case 0:
off=(u32)(158+end_angle*2.2);
pca_setpwm(num,0,off);
break;
case 1:
off=(u32)(158+end_angle*2.2);
pca_setpwm(num,0,off);
if(end_angle>start_angle){delay_ms((u16)((end_angle-start_angle)*2.7));}
else{delay_ms((u16)((start_angle-end_angle)*2.7));}
break;
case 2:
if(end_angle>start_angle)
{
for(i=start_angle;i<=end_angle;i++)
{
off=(u32)(158+i*2.2);
pca_setpwm(num,0,off);
delay_ms(2);
delay_us(speed*250);
}
}
else if(start_angle>end_angle)
{
for(i=start_angle;i>=end_angle;i--)
{
off=(u32)(158+i*2.2);
pca_setpwm(num,0,off);
delay_ms(2);
delay_us(speed*250);
}
}
break;
}
}
main函数代码
#include "led.h"
#include "delay.h"
#include "key.h"
#include "sys.h"
#include "usart.h"
#include "oled.h"
#include "pca9685.h"
#include "stdlib.h"
#include "string.h"
#include "pid.h"
#include "stdio.h"
extern float X_Angle,Y_Angle;
extern char X_Angle_arr[4],Y_Angle_arr[4];
u16 Len_Buf;
char X[10],Y[10];
extern PID pid;
/***********************************************
PID调节参数
1.对于增量式PID首先需要调整的是I参数，I参数应该从小到大去设置
如何算是完美的I参数，如果当I参数设置到两个舵机只震荡一到两次即可证明I参数设置好
2.然后设置P参数，P参数的设置需要将I参数下调到80%,然后大到小的设置，这里我是凭借经验设置的
3.D参数可加可不加，因此我设置的比较小为0.001
***********************************************/
void PID_Init()
{
pid.Set_X_Location = 160; //设置中心位置
pid.Set_Y_Location = 120; //设置中心位置
pid.Current_X_Location = 0; //当前位置
pid.Current_Y_Location = 0;//当前位置
pid.Kp = 0.01; //比例系数
pid.Ki = 0.06; //积分时间常数
pid.Kd = 0.001; //微分系数
pid.Error_X_Current = 0; //当前偏差
pid.Error_Y_Current = 0; //当前偏差
pid.Error_X_Last = 0; //上一次偏差
pid.Error_Y_Last = 0; //上一次偏差
pid.Error_X_Last_Last = 0; //上一次偏差
pid.Error_Y_Last_Last = 0; //上一次偏差
pid.Output_X_Current = 307;
pid.Output_Y_Current = 307;
pid.Integral_X_Out = 0; //积分输出
pid.Proportion_X_Out = 0; //比例输出
pid.Derivative_X_Out = 0; //微分输出
pid.Integral_Y_Out = 0; //积分输出
pid.Proportion_Y_Out = 0; //比例输出
pid.Derivative_Y_Out = 0; //微分输出
pid.Output_X = 0;
pid.Output_Y = 0;
pid.Output_X_PWM = 0;
pid.Output_Y_PWM = 0;
pid.Last_X_PWM = 0;
pid.Last_Y_PWM = 0;
}
void Data_Process(char *target,u8 *source,int n)
{
int count;
for(count=0;count<n;count++)
{
*target++=*source++;
}
}
void Receive_Process()
{
u16 i;
u16 Separator;
if(USART_RX_STA&0x8000)
{
if((USART_RX_BUF[0]==0XAA)&&(USART_RX_BUF[1]==0XAA))
{
Len_Buf = strlen((char*)USART_RX_BUF);
for(i=2;i<Len_Buf;i++)
{
if(USART_RX_BUF[i] == ',')
{
Separator=i;
}
}
Data_Process(X,USART_RX_BUF+2,Separator-2);
Data_Process(Y,USART_RX_BUF+Separator+1,Len_Buf-Separator);
pid.Current_X_Location=atoi(X);
pid.Current_Y_Location=atoi(Y);
if(abs(pid.Current_X_Location-160)<5&&abs(pid.Current_Y_Location-120)<5)
{
LED0=!LED0;
pca_setpwm(0,0,pid.Last_X_PWM);
X_Angle = 0.439 * (pid.Last_X_PWM-102);
sprintf(X_Angle_arr,"%4.2f",X_Angle);
OLED_ShowString(64,0,(uint8_t *)X_Angle_arr,16);
pca_setpwm(1,0,pid.Last_Y_PWM);
Y_Angle = 0.439 * (pid.Last_Y_PWM-102);
sprintf(Y_Angle_arr,"%4.2f",Y_Angle);
OLED_ShowString(64,4,(uint8_t *)Y_Angle_arr,16);
}
else
{
Pid_Calcuate();
}
// printf("%f\r\n",pid.Current_X_Location);
// printf("%f\r\n",pid.Current_Y_Location);
memset(USART_RX_BUF,0,sizeof(USART_RX_BUF));
memset(X,0,sizeof(X));
memset(Y,0,sizeof(Y));
USART_RX_STA=0;
}
else
{
memset(USART_RX_BUF,0,sizeof(USART_RX_BUF));
USART_RX_STA=0;
}
}
}
int main(void)
{
// u16 i;
delay_init(); //延时函数初始化
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //设置NVIC中断分组2:2位抢占优先级，2位响应优先?
uart_init(115200);
KEY_Init();
LED_Init(); //LED端口初始化
OLED_Init() ;
OLED_Clear() ;
display();
PID_Init();
IIC_Init(); //IIC初始化
pca_write(pca_mode1,0x0); //写地址
pca_setfreq(50); //写舵机频率，模拟电机给50HZ，数字电机330HZ
pca_setpwm(0,0,307);
pca_setpwm(1,0,307);
X_Angle = 0.439 * (307-102);
sprintf(X_Angle_arr,"%4.2f",X_Angle);
OLED_ShowString(64,0,(uint8_t *)X_Angle_arr,16);
Y_Angle = 0.439 * (307-102);
sprintf(Y_Angle_arr,"%4.2f",Y_Angle);
OLED_ShowString(64,4,(uint8_t *)Y_Angle_arr,16);
while(1)
{
Receive_Process();
}
}
OpenMV的代码
帧头为0XAA和0XAA，然后是两个坐标值，再就是0X0D和0X0A（\r\n）
# main - By: DN36 - 周四 1月 28 2021
import sensor, image, time
from pyb import UART
#red_threshold = (13, 49, 18, 61, 6, 47)
red_threshold = (26, 100, 36, 127, -128, 127)
pan_current = 0
tilt_current = 0
FH = bytearray([0XAA,0XAA])
sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.RGB565) # use RGB565.
sensor.set_framesize(sensor.QVGA) # use QVGA for speed.
sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_whitebal(False) # turn this off.
clock = time.clock() # Tracks FPS.
uart = UART(3, 115200, timeout_char=1000)
uart.init(115200, bits=8, parity=None, stop=1)
def find_max(blobs):
max_size=0
for blob in blobs:
if blob[2]*blob[3] > max_size:
max_blob=blob
max_size = blob[2]*blob[3]
return max_blob
while True:
clock.tick() # Track elapsed milliseconds between snapshots().
img = sensor.snapshot() # Take a picture and return the image.
blobs = img.find_blobs([red_threshold])
if blobs:
max_blob = find_max(blobs)
# pan_error = max_blob.cx()-img.width()/2
# tilt_error = max_blob.cy()-img.height()/2
x=max_blob.cx()
y=max_blob.cy()
data = "%d,%d" %(x,y)
uart.write(FH)
uart.write(data+'\r\n')
print("pan: ", img.width())
print("tilt: ", max_blob.cy())
img.draw_rectangle(max_blob.rect()) # rect
img.draw_cross(max_blob.cx(), max_blob.cy()) # cx, cy
在此感谢一下CSDN博主番杰的一些帮助，https://blog.csdn.net/JIE15164031299，这是他的博客，他写的比我更详细，希望大家关注一下！
