#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]為第三次的偏角
}