|
1. //***************************************************** 2. //定義PID結構體 3. //***************************************************** 4. typedef struct PID 5. { 6. int SetPoint; //設定目標Desired Value 7. double Proportion; //比例常數Proportional Const 8. double Integral; //積分常數Integral Const 9. double Derivative; //微分常數Derivative Const 10. int LastError; //Error[-1] 11. int PrevError; //Error[-2] 12. } PID; 13. //***************************************************** 14. //定義相關宏 15. //***************************************************** 16. #define P_DATA 100 17. #define I_DATA 0.6 18. #define D_DATA 1 19. #define HAVE_NEW_VELOCITY 0X01 20. //***************************************************** 21. //聲明PID實體 22. //***************************************************** 23. static PID sPID; 24. static PID *sptr = &sPID; 25. //***************************************************** 26. //PID參數初始化 27. //***************************************************** 28. void IncPIDInit(void) 29. { 30. sptr->LastError = 0; //Error[-1] 31. sptr->PrevError = 0; //Error[-2] 32. sptr->Proportion =P_DATA; //比例常數Proportional Const 33. sptr->Integral =I_DATA; //積分常數Integral Const 34. sptr->Derivative =D_DATA; //微分常數Derivative Const 35. sptr->SetPoint =100; 目標是100 36. } 37. //***************************************************** 38. //增量式PID控制設計 39. //***************************************************** 40. int IncPIDCalc(int NextPoint) 41. { 42. int iError, iIncpid; //當前誤差 43. iError = sptr->SetPoint- NextPoint; //增量計算 44. iIncpid =sptr->Proportion * iError //E[k]項 45. - sptr->Integral * sptr->LastError //E[k－1]項 46. + sptr->Derivative * sptr->PrevError; //E[k－2]項 47. sptr->PrevError =sptr->LastError; //存儲誤差，用于下次計算 48. sptr->LastError =iError; 49. return(iIncpid); //返回增量值 50. }
| 
