2021-09-11

openmv+数字舵机+匿名上位机 +数字舵机二维云台
本设计是以STM32F407为主控板,通过openmvH4摄像头识别目标的中心,传送给单片机,事先设定好以摄像头识别的范围的中点作为参照点(x,y)。在二维坐标系里只要确定物体的中心坐标的(X0,Y0)坐标,分别对X0或者Y0坐标的距离参照物的x或者y坐标的距离进行缩减,过程才用到PID的算法。
对于PID算法,先进行P的调试(将近接近目标值),再对PD进行调试(减少震荡),最后加人I项(使停在目标值处)。
在此过程中需要弄懂openmvH4和是他们2f407的通信协议。知道两者的数传输的格式。还有就是PID的参数调节,可以借助匿名上机的帮助,可以很大的减少时间。

PID的程序
#include “pid.h”
#include “stdio.h”
#include “control.h”

#define MODEL_P 1
#define MODEL_PI 2
#define MODEL_PID 3
PID pid;
float out;
// u8 pid_10ms; // 用来控制运算周期的时间;

void PID_Init(void)
{
//以下是X轴最好的参数了
pid.choose_model= MODEL_PID;//P:9.0借助匿名上位机来调节P/I/D会成功很多
pid.X_Kp = 9.00f;
pid.X_Ki = 0.1f;
pid.X_Kd = 0.3f;

// pid.Ek = 0;
// pid.Ek1 = 0;
//
pid.Y_Kp = 9.0f;
pid.Y_Ki = 0.3f;
pid.Y_Kd = 0.3f;

}

void PID_Calc_X(void)
{

/*PID计算式/
pid.Sv=159;
pid.Ek = pid.Sv - pid.Pv; // 当前偏差值;
// printf(“pid.Sv:%f\r\n”,pid.Sv);
// printf(“pid.Ek:%f\r\n”,pid.Ek);
pid.SEk += pid.Ek; // 历史偏差之和;
// printf(“pid.SEk:%f\r\n”,pid.SEk);
pid.AdEk = pid.Ek - pid.Ek1; // 相邻之差;
// printf(“pid.AdEk:%f\r\n”,pid.AdEk);

pid.Pout = pid.X_Kp * pid.Ek;		// P: 比例输出;

// printf(“pid.Pout:%f\r\n”,pid.Pout);
pid.Iout = pid.X_Ki * pid.SEk; // I: 积分输出;
// printf(“pid.Iout:%f\r\n”,pid.Iout);
pid.Dout = pid.X_Kd * pid.AdEk; // D: 微分输出;
// printf(“pid.Dout:%f\r\n”,pid.Dout);

switch(pid.choose_model)
{
	case MODEL_P:  out = pid.Pout;break;
	case MODEL_PI: out = pid.Pout+pid.Iout;break;
	case MODEL_PID:out = pid.Pout+ pid.Iout + pid.Dout;break;
}		

// printf(“outX:%f\r\n”,out);

}

void PID_Calc_Y(void)
{

/*PID计算式/
pid.Sv=119;
pid.Ek = pid.Sv - pid.Pv; // 当前偏差值;
// printf(“pid.Sv:%f\r\n”,pid.Sv);
// printf(“pid.Ek:%f\r\n”,pid.Ek);
pid.SEk += pid.Ek; // 历史偏差之和;
// printf(“pid.SEk:%f\r\n”,pid.SEk);
pid.AdEk = pid.Ek - pid.Ek1; // 相邻之差;
// printf(“pid.AdEk:%f\r\n”,pid.AdEk);

pid.Pout = pid.Y_Kp * pid.Ek;		// P: 比例输出;

// printf(“pid.Pout:%f\r\n”,pid.Pout);
pid.Iout = pid.Y_Ki * pid.SEk; // I: 积分输出;
// printf(“pid.Iout:%f\r\n”,pid.Iout);
pid.Dout = pid.Y_Kd * pid.AdEk; // D: 微分输出;
// printf(“pid.Dout:%f\r\n”,pid.Dout);

switch(pid.choose_model)
{
	case MODEL_P:  out = pid.Pout;break;
	case MODEL_PI: out = pid.Pout+pid.Iout;break;
	case MODEL_PID:out = pid.Pout+ pid.Iout + pid.Dout;break;
}		

// printf(“out:%f\r\n”,out);

}

void READ_Angle( float temp) //从传感器读回数据
{
pid.Pv = temp;
// printf(“pid.Pv:%f\r\n”,pid.Pv);
}

你可能感兴趣的:(c语言,stm32,算法)