setover_pid.c

飞行控制中的PID计算

#include<p30f4013.h>
#include<math.h>
#include<stdio.h>
static EPJ[3];
float delta_uk;
void setover_pid(float d)
{
	float A,B,C,KP,TI,TD,T;
//	EPJ[0]=EPJ[1]=0;							//偏距初始值为0
	EPJ[2]=d;									//获取偏距
	KP=0.7;
	TI=0.8;
	TD=2.4;										//设定PID运算的参数
	T=0.25;										//根据奈奎斯特定律，设定周期为0.25s，频率为4Hz，大于有用信号的频率1Hz的两倍
	A=KP*(1+T/TI+TD/T);
	B=KP*(1+2*TD/T);
	C=KP*TD/T;
	delta_uk=A*EPJ[2]-B*EPJ[1]+C*EPJ[0];		//计算增量式PID运算的增量
	EPJ[0]=EPJ[1];
	EPJ[1]=EPJ[2];								//后处理，EPJ[0]为第一次的偏距，EPJ[1]为第二次的偏距，EPJ[2]为第三次的偏距
}