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