PID Speed Control

https://4rdp.blogspot.com/2008/05/pid-speed-control.html

LEGO MINDSTORMS NXT 的控制器能夠同時驅動三組伺服馬達，
伺服馬達與一般馬達主要差異在它多了回授系統。
LEGO Servo Motor 能夠回報馬達轉動角度訊息給控制器，
並且可以像步進馬達一樣控制旋轉角度，解析達 1度角，這真是不錯的設計！

本文先討論馬達轉速控制，
NXT 玩家們控制馬達時，應該會發現用 NXT 控制馬達的角度或轉數蠻容易，所以用 NXT 精確馬達定位，不成問題。
但是想控制馬達定速運轉，這不是一件容易的事，
因為 NXT 使用不同電池或者換個地點 (負載狀況不同)，相同的程式產出的結果會不同。


馬達轉速慣用單位為 rpm (Revolution(s) Per Minute，每分鐘轉數)，NXT馬達轉速不高，Power = 100，全速運轉時約 133 rpm。
使用 NXC (BricxCC) 程式，可以每秒計數 NXT 馬達轉角，所以 133 rpm = 133 x 360° / 60 seconds = 798° / second。

我使用六顆一般充電電池，充飽電後全速可達 850° / second，但是快沒電時，全速運轉約 750° / second。

現在介紹 PID Control，屬於系統控制的領域，這方法可消弭一些外界變異或干擾影響，
讓系統控制可預測，參數設定得好，就可以又快又穩又準。

PID (Proportional Integrative Derivative)

E(t) = R(t) – Y(t) // a set point R(t) to reach, measuring value Y(t)

P(t) = E(t)
I(t) = I(t-1) + E(t)
D(t) = E(t) – E(t-1)

U(t) = Kp*P(t) + Ki*I(t) + Kd*D(t)

以上為一般方程式，適用於任何可控制回授系統，
而下列兩式為 NXT 馬達特有方程式。

Power = U(t) / 8
Y(t) = U(t-2)

U(t) 為系統估算出的轉速，依據實驗數據所知，Power = 100 時，全速轉速約為 800° / second，因此 Power = U(t) / 8。
而 Y(t) = U(t-2)，是因為 NXT馬達轉速回應有一秒延遲的現象 (1 second deadtime)。

有興趣的朋友試試這個程式，只用馬達 A 不需組裝：

　
#include "NXCDefs.h"
　
#define Kp 40
#define Ki 40
#define Kd 0
　
task main(){
long power;
long set_speed,speed;
long e0,e1;
long x0,x1;
long P,I,D,U;
long time;
string str;
　
ResetAllTachoCounts(OUT_A);
x0 = 0;
e0 = 0;
power = 0;
set_speed = 500;   // degree/second
I = 0;
time = 0;
　
while (1) {
OnFwdRegEx(OUT_A, power, OUT_REGMODE_IDLE, RESET_NONE);
　
x1 = MotorRotationCount(OUT_A);
Wait(1000);                  // wait 1 second
speed = x1 - x0;
x0 = x1;
　
e1 = set_speed - speed;
　
P = e1;
I = I + e1;
D = e1 - e0;
e0 = e1;
　
U = (Kp*P + Ki*I + Kd*D) / 100;
power = U / 8;
if (power＞100)
power = 100;
else if (power＜-100)                  
power = -100;            
　
str = NumToStr(speed);   
str = StrCat("Speed = ",str);   
TextOut(0, LCD_LINE1, str, true);     
　
str = NumToStr(time);   
str = StrCat("t = ",str);   
TextOut(0, LCD_LINE3, str, false);   
time += 1;   
str = NumToStr(P);   
str = StrCat("P = ",str);   
TextOut(0, LCD_LINE4, str, false);   
str = NumToStr(I);   
str = StrCat("I = ",str);   
TextOut(0, LCD_LINE5, str, false);   
str = NumToStr(D);   
str = StrCat("D = ",str);   
TextOut(0, LCD_LINE6, str, false);     
　
str = NumToStr(power);   
str = StrCat("Power = ",str);   
TextOut(0, LCD_LINE8, str, false);
}
}
　

PS.為了讓程式碼排版正常，程式中使用了全形空格當空行以及全形＜＞，編譯前請留意修改。

另外附上 線上試算表可以模擬試算，電機系學自動控制的同學，可以藉由這個實驗把 PID 搞懂。
最後提醒各位，網路上有許多 PID control 文章，大家對一般方程式的定義有些許不同，
所以Kp、Ki、Kd數值沒搞清楚定義前不要亂用一通，通常使用 0 <= K <= 2。 NXT 馬達反應速度並不快，因此 Kd = 0 就可以了。
本程式的 K值很大，是因為 NXC 無法使用小數，所以 K = 40，代表 K = 40 / 100 = 0.4。

後記補充，另一篇 Apply PID to control speed and angle 有比較速度與角度控制差異。