飛思卡爾控制電機PID算法代碼解析

PID實指“比例proportional”、“積分integral”、“微分derivative” , 這三項構成PID基本要素。每一項完成不同任務��對系統功能產生不同的影響。 搞軟件的往往對硬件不屑 ,卻忘了軟件再牛B也是為硬件服務的 , PID的數字化算法就完全是為硬件而生的 , 這是控制電機算法的一個難點 , 沒有一定的對軟硬件的理解 , 就連調試裝定PID參數都會很為難 ! 飛思卡爾智能車項目里面就會用到PID算法 , 比如車爬坡和平地連續拐彎時 , 代碼里面沒有PID算法 , 控制和驅動起來就會很拙急 , 對不對 ? 不過 ,搞清楚問題還是有個前提的 , 那就是懂點微積分 , 不會微積分嘛 , 下面的內容無需關注

實際上 , PID算法的應用基礎始于對PCB板上的運放的PID參數進行調校 , P對應于運放增益  ; I 就是運放輸入和輸出端之間接一個電容引入反饋 , 就是控制器的輸出與輸入誤差會積累起來影響輸出 ;  D 就是運放輸入端串接一個電容 ,起的微分作用是阻止輸出與輸入誤差的變化 .  結合示波器來觀察控制電機的PID參數設定的話 , 網上有一首詩 , 它就代表我的心聲了 :

參數整定找最佳，從小到大順序查
先是比例后積分，最后再把微分加
曲線振蕩很頻繁，比例度盤要放大
曲線漂浮繞大灣，比例度盤往小扳
曲線偏離回復慢，積分時間往下降
曲線波動周期長，積分時間再加長
曲線振蕩頻率快，先把微分降下來
動差大來波動慢，微分時間應加長
理想曲線兩個波，前高后低四比一
一看二調多分析，調節質量不會低 !

下面貼段代碼 (真的是僅供參考):

1.     #include <mc9s12dg128.h> /* derivative information */
   
2.     /*******************************************************************
   
3.     * 宏定義
   
4.     **********************************************************************************/
   
5.     #define STABMAX 50
   
6.     #define SENSORNUM 8
   
7.     #define SAMPLETIMES 5
   
8.     /********************************************************************
   
9.     FUNCTION PROTOTYPES
   
10.     ********************************************************************/
    
11.     int CalculateP(void);
    
12.     float CalculatePID(void);
    
13.     /********************************** PID控制程序 *******************/
    
14.     struct CARSTATE
    
15.     {
    
16.     int E0;
    
17.     int E1;
    
18.     int E2;
    
19.     int E3;
    
20.     float Integral;
    
21.     }CarState;
    
22.     /*
    
23.     ********************************************************************
    
24.     * 初始化PID參數
    
25.     ********************************************************************/
    
26.     void Init_PID()
    
27.     {
    
28.     CarState.E0 = 0;
    
29.     CarState.E1 = 0;
    
30.     CarState.E2 = 0;
    
31.     CarState.E3 = 0;
    
32.     CarState.Integral = 0;
    
33.     }
    
34.     /*
    
35.     ********************************************************************
    
36.     * 信號處理函數
    
37.     * 程序描述: 對傳感器采集過來的數據進行處理，得到一些基本的計算參數
    
38. 
    
39.     ********************************************************************/
    
40.     int SignalProcess( unsigned int signal )
    
41.     {
    
42.     const int BitValue[8] = {43,26,12,6,-6,-12,-26,-43}; //MAX:28
    
43.     int i,CurrPoint=0,LastPoint=0,BitNum=0;
    
44.     unsigned char SignalBit[8];
    
45.     for(i=0;i<8;i++)
    
46.     {
    
47.     SignalBit = signal & 0x0001;
    
48.     BitNum += SignalBit;
    
49.     signal >>= 1;
    
50.     }
    
51.     switch(BitNum)
    
52.     {
    
53.     case 1:
    
54.     for(i=0;i<8;i++)
    
55.     if(SignalBit != 0)
    
56.     CurrPoint += BitValue;
    
57.     CarState.E0 = CurrPoint;
    
58.     break;
    
59.     case 2:
    
60.     for(i=0;i<8;i++)
    
61.     if(SignalBit != 0)
    
62.     CurrPoint += BitValue;
    
63.     CurrPoint >>= 1;
    
64.     CarState.E0 = CurrPoint;
    
65.     break;
    
66.     default:
    
67.     CarState.E0 = CarState.E1;
    
68.     break;
    
69.     }
    
70.     return CalculateP()*100;
    
71.     }
    
72.     /*******************************************************************
    
73.     * PID計算函數
    
74. 
    
75.     * 程序描述: 計算P參數
    
76. 
    
77.     ********************************************************************/
    
78.     int CalculateP(void)
    
79.     {
    
80.     CarState.E1 = CarState.E0;
    
81.     return((int)CarState.E0);
    
82.     }
    
83. 
    
84.     /*
    
85.     ***********************************************************************************
    
86.     * PID計算函數
    
87.     *
    
88.     * 程序描述: 對傳感器采集過來的數據進行處理，得到一些基本的計算參數
    
89.     *
    
90.     ******************************************************************* ***************/
    
91.     float CalculatePID(void)
    
92.     {
    
93.     float P, I = 0, D;
    
94.     /* parameter const */
    
95.     float Kp = 1.0, Ki = -0.0002, Kd = -0.0002;
    
96.     /* P parameter */
    
97.     P = CarState.E0 * Kp;
    
98.     /* I parameter */
    
99.     if(P+I<2)
    
100.     {
     
101.     CarState.Integral += Ki * CarState.E0;
     
102.     I = CarState.Integral;
     
103.     }
     
104.     /* D parameter */
     
105.     D = Kd * ( CarState.E0 + 3*CarState.E1 - 3*CarState.E2 - CarState.E3 )/6.0;
     
106.     CarState.E3 = CarState.E2;
     
107.     CarState.E2 = CarState.E1;
     
108.     CarState.E1 = CarState.E0;
     
109.     return (P+I+D);
     
110.     }
     

復制代碼

不錯。很實用

不錯，很實用