//==============================================================
//PID.c文件
//==============================================================
#include?“DSP28x_Project.h“?
#include?“C28x_FPU_FastRTS.h“
#include?

#include?“PID.h“

//========函數定義?===========================
//**********************************
/*
??@?Description:
??@?Param
??@?Return
*/
//**********************************
void?PIDfunc_calc（PID_FUNC?*p）
{	
?//使用條件編譯指令進行切換
?#if?PID_DEBUG??//在校正PID參數時，使用宏定義將PID_DEBUG設為1，從而執行以下程序
???float?a0a1a2;
??
???//這里每次都要計算?a0、a1、a2的值
???a0?=?p->Kp*（1?+?p->T/p->Ti?+?p->Td/p->T）;
???a1?=?p->Kp*（1?+?2*p->Td/p->T）;
???a2?=?p->Kp*p->Td/p->T;
???
???//計算PID調節器的輸出
???p->Output?=?p->LastOutput?+?a0*p->Ek?-?a1*p->Ek_1?+?a2*p->Ek_2;
???
?#else??//當參數校正完成后，那么得到固定的a0、a1、a2的值，使用宏定義將PID_DEBUG設為0，從而執行以下過程
????//當參數校正完成后，初始化時直接為p->a0、p->a1、p->a2賦值，省去計算過程
p->Output?=?p->LastOutput?+?p->a0*p->Ek?-?p->a1*p->Ek_1?+?p->a2*p->Ek_2;
?#endif

??//輸出限幅
???if（p->Output?>?p->OutMax）?p->Output?=?p->OutMax;
???if（p->Output?OutMin）?p->Output?=?p->OutMin;

??//保存上一周期的值
???p->LastOutput?=?p->Output;
???p->Ek_1?=?p->Ek;
???p->Ek_2?=?p->Ek_1;		
}
//==============================================================
//End?of?file.
//==============================================================