angle_pid.c

采用C语言开发的16位单片机dsPIC

#include<p30f4013.h>
#include<math.h>
#include<stdio.h>
static EAP[3];									//EAP[3]为3次偏角数组
float delta_uk_an
void angle_PID(float AN)
{
	float A,B,C,KP,TI,TD,T;
	//EAP[0]=EAP[1]=0;							//偏角初始值为0
	EAP[2]=AN;									//获取偏角
	KP=0.8;
	TI=8;
	TD=2;										//设定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;									//根据参考文献《新型PID控制及其应用》P6计算参数
	delta_uk_an=A*EAP[2]-B*EAP[1]+C*EAP[0];		//计算增量式PID运算的增量
	EAP[0]=EAP[1];
	EAP[1]=EAP[2];								//后处理，EAP[0]为第一次的偏角，EAP[1]为第二次的偏角，EAP[2]为第三次的偏角
}