Software PID Control of

an Inverted Pendulum
Using the PIC16F684
Author : John Charais
Ruan Lourens
Microchip Technology
Inc.
論文研讀：蕭鈞煒
 基座
 為三點接觸，包含二輪 ( 其中一輪安裝 DC
馬達及齒輪組作為驅動輪 ) 及一個 audio
jack 當作旋轉軸及提供電源。
 單擺
 單擺透過一 360° 的電位計與基座連接，當
平衡時為垂直並位於電位計中點且為 VREF/2 。
並以 θ表示為單擺角位移與垂直軸的關係
。
控制板
 控制板
 量測 θ
 驅動馬達
 根據馬達選擇正確的電源供應
 由 PIC16F684
Enhanced Capture/compare/PWM Module
(ECCP) 驅動 H-bridge 控制馬達
 ECCP 驅動 FET 產生適當的驅動電壓並減
少 H-bridge FET 的轉換時間
 控制板
 有 5 個電位計位於控制器 ，其中 3 個用
來調整的 PID 常數 （ KP, KI and KD ）和
1 個用來量測 θ。第 5 電位計連接至輸入
濾波器參考端。輸入濾波器是一個低通
Bessel 濾波器具截止頻率為 60 赫茲及電
壓增益 6 。低通濾波器用來消除角度測
量時的高頻雜訊。選擇 Bessel filter ，因
為它有步階函數的最佳響應。 ( 當單擺
平衡時突然的位移干擾近似步階函數 )
 控制板
 截止頻率應選擇至少兩倍單擺的預期頻
率。 Filter 的增益用來增加 A/D converter 的解
析度。隨著 360° 電位和 10 位 A / D 轉換器，
沒有增益，每一 LSb 為 0.35° 。當增益設置為
6 ，位移角度被限制在 ± 30° ，每一 LSb 為
0.059° 。第 5 電位計控制輸入濾波器的參考產
生一個當擺角垂直時的真正 0° 位移，。如果沒
有這個電位計，任何輕微的偏移角，會導致基
板緩慢加速，使系統變成不穩定狀態。
PID
 PID
 平衡時， R(t) 為 θ=0°
 Eq.1 ：
t dE (t )
C (t )  K P E (t )  K I  E (t )dt  K D
0 dt
 PID IN A DIGITAL SYSTEM
 Y(t) 由 A/D converter 量測，為了要實現
PID 控制器，必須作近似的積分和微分項。
 Eq.2 ：
dE (t )
 E (n)  E (n  1)  / TS
dt
 Eq.2 為近似 E(t) 切線的斜率
 PID IN A DIGITAL SYSTEM
 Eq.3
N
E (t )dt TS  E (n)
t
0
0

 Eq.4
N
C (n)  K P E (n)  K I TS  E (n)  K D  E (n)  E (n  1)  / TS
0

OR
N
C (n)  K ( E (n)  (TS / TI ) E (n)  (TD / TS )  E (n)  E (n  1)  )
0

Where K P =K, K I =K/TI & K D =KTD

 倒單擺建模
 倒單擺建模需注意的變數
基座的位置、速度、加速度、轉動慣量、
摩擦係數、質量、長度
地心引力
單擺的位置、速度、轉動慣量、摩擦係數
、質量、長度
 依照經驗法則來簡化這些變數，馬達的選
擇要能加速基座且要比單擺掉下的速度還
要快
 倒單擺建模
 單擺的角加速度與角位移的關係為
Eq.5
   ( g / R)
 單擺的加速不是常數，依照經驗以最大加速
度 ( 發生在 θMAX) ，在硬體的 θMAX 是由 Bessel
filter 的增益決定 (gain=6 → θ=±30°) 。
更進一步由軟體限制 (θMAX=20°) ，這可消除
在硬體邊界所產生的 hitting 現象
 EX ：馬達的選擇
 Pendulum length = 0.5 m
 θMAX=20°(0.349 rad)
from Eq.5
  ( g / R)
 MAX
  (9.81/ 0.5)0.349
 MAX
  6.6845 rad / sec 2
 MAX

 MAX  3.425 m / sec 2
 EX ：馬達的選擇
 根據前述條件， MAXX 系列的 EPU9 的馬達符合
條件，設定齒輪比 8.6 去驅動 2 inch 的輪，馬達
的額定電壓 4.8~7.2 V ，但這個馬達會在 12 V 的
環境下使用
★ at small duty cycles from the PWM 可獲得更好
的響應
★ Running at approximately double the designated
input voltage is not a concern because the motor will
never be in a constant state where the duty cycle of
the PWM is greater than 50%
 PID control loop update
 在 Eq.3 中的積分項可看出誤差須乘上取樣
時間 ( 相當於除以取樣頻率 ) ，選擇取樣頻
率的二倍時， a very fast divide routine can
be done by using the right shift command 。
 選擇 256 Hz 的取樣頻率，預測最低頻率快
了 8 倍，這允許在需要時可加長單擺的長
度。
 C CODE
Main Routine
FLOW
NO
CHART
Reset of PID Flag Limit if too big
PIC16F684 Set ? or small

YES
Initialization Calculate Sum terms &
Routine Integral Term mult. By KP

Limit if too big

Read KP, KI KD Read KP, KI KD
or small

Calculate
Read KP, KI KD
Deriv. Term
 Interrupt Service Routine
Error to YES
Reset TMR0
Big ?

NO
Read Error
Angle YES
Error to Clear PID
Small ? Loop Flag

Set PID Loop NO

Flag
Stop PWM,
Return
Reset terms
 SUBTLETIES IN THE
SOFTWARE
 Interrupt service routine is used to control the
speed of the PID loop
 Timer0 is an 8 bit timer that increment the
TMR0 register every instruction clock
→ 3.9ms(1/256 Hz)
 8 MHz → 2MHz instruction clock → 0.5μs
→ 3.9ms = 7812 instruction
 SUBTLETIES IN THE
SOFTWARE
 setting the TMR0 prescaler to 32 and
reloading 11 into the TMR0 register in the
interrupt service routine
 The Timer0 interrupt occurs every 3.9
milliseconds or every 7808 instructions (255-
11)*32
 SUBTLETIES IN THE
SOFTWARE
 二種基本型式的 PID 可以實現
Eq6.
N
C  n   K P E  n   K I TS  E  n   K D  E  n   E  n  1  / TS
0

or
N
C  n   K ( E  n   (1/ TI ) E  n   TD  E  n   E  n  1  / TS
0

Where K P  K, K I  K / TI & K D  KTD

 SUBTLETIES IN THE
SOFTWARE
 如果預期電源電壓會變化如電池供電的應
用則應使用後式，因比例常數的變化不會
影響控制器極響應。其可改變比例常數，
以補償電源供應的變化。比例常數與馬達
的供應電壓成反比 。
 SUBTLETIES IN THE
SOFTWARE
 為了讓不穩定系統進入穩定導數項是關鍵。在任
何 PID 控制導數項作用如同預估器。經由比對當
前誤差與前一誤差，控制器就可以判斷出誤差項
是越來越大或變小。
如果誤差項是越來越大，導數項加進控制器輸出
如同比例和積分計算，但影響較小。如果誤差項
是越來越小，超過預估的超越量時控制器輸出將
減去導數項。如果沒有導數項的系統將永遠是不
穩定的，因為沒有辦法補償超越的情況。下面是
實際的 C 代碼用來計算導數項
//Calculate the differential term
derivative_term = en0 - en3;
if(derivative_term > 120){
derivative_term = 120;
}
if(derivative_term < -120){
derivative_term = -120;
}
derivative_term = derivative_term * kd;
derivative_term =derivative_term>>5;
//divide by 32
}
if(derivative_term > 120){
derivative_term = 120;
}
if(derivative_term> -120){
derivative_term = -120;
}
 SUBTLETIES IN THE
SOFTWARE
 當近似導數項，也就是
Eq7.
K D dE (t ) / dt  TD [ E (n) - E (n -1)] / TS
Where TD = KD/KP
 C CODE 是來近似上式，但實際上為
Eq8.
K D dE (t ) / dt  KD [ E (n) - E (n - 3)] / 32
 SUBTLETIES IN THE
SOFTWARE
 更準確的寫法為
Eq9.
K D dE (t )  TD [ E (n) - E (n - 3)] / ( X 3TS )
 K D [ E (n) - E (n - 3)] / ( K P X 3TS )
Where X is an unknown scaling factor
 SUBTLETIES IN THE
SOFTWARE
 在 10 位 A / D 轉換，量測到的 KD 為 0 至 1023 的整
數。 X 項允許使用分數表示 KD ，非限定為整數。（與積
分和比例計算相似）。 3 * TS 則是因為 3 倍採樣週期由使
用 E(n) – E(n-3) 而不是 E(n) – E(n-1) ，最主要的原因限
制角度測量誤差變化。因為總是有誤差測量的不確定量，
其中一些可歸咎於 A / D 轉換誤差， Bessel filter
throughput ，機械振動等，由於不確定性的測量誤差如同
的所有誤差項，採樣週期的三倍可有效地降低了這種不確
定性。真正的關鍵是，導數項仍然是更新率在 256 赫茲。
這樣做的好處是讓速度較慢的導數項更準確，但在期望更
快的採樣率情況下。這樣做的副作用是增加約 4 毫秒滯後
的導數項，而在這種情況下是可以接受的。 KP*X*3*TS
預先計算為 32 增加 PID 迴路速度。
TUNING THE PID CONTROLLER
1. 轉動 KP 、 KI 和 KD 電位計逆時針方向至底。這
設置了所有的常數為零。
2. 供應 12V 電源至設備 ( 至少 3A)
3. 保持按下 RESET ，將單擺移至垂直位置並放開
RESET
4. 單擺應掉下且基座不移動，並確認所有常數應為
零
5. 轉動電位計增加 KP 並重復 STEP 3
6. 持續 STEP 3~5 ，基座應輕微振盪，若 KP 太小，
基座將隨單擺傾斜角移動追趕，若太大則沒煞車
或高速振盪。
TUNING THE PID CONTROLLER
7. 開始增加 KI 如同 KP 直到幾秒後單擺能平衡， KI
增加可以增加追趕的加速性，此時單擺可能往
反方向傾倒，基座此時應立即往反方向趕，如
此重復，這就是 over shoot 情況
8. 加入 KD 使 over shoot 情況改善並保持單擺平衡
9. 直到 over shoot 情況消失，完成 PID 調整
 CONCLUSION
 通過使用 PIC16F684 的 ECCP 和 A / D 模塊
，展現出如何實現位置 PID 控制器，使系
統成為一個內在的不穩定性穩定。執行關
鍵是控制有一個基本的了解機械系統，並
確定導數項這個關鍵因素，在整體穩定的
制度。其他關鍵像是軟體用來確保我們永
遠不會溢出寄存器， PID 迴路的頻率和挑
選讓我們可以使用左，右移位來達到快速
乘法和除法。