實際編程

昨天介紹了PID的理論與原理，最後以下面這個公式收尾

但我們到底要怎麼在程式當中積分、微分呢？
沒錯，我們的確沒有辦法直接使用這個公式，對我們來說，我們只能以離散的形式來做計算，而上面的式子經過轉換後就會變成下面這個離散的形式。

雖然很多人會覺得這個式子並沒有比剛剛的好多少啊，而且還多冒出來一個Sigma的符號。事實上，這已經可以讓我們在程式當中實做出來了，它把積分轉換成為連加，而微分轉換成為誤差差值除以時間，這些都是基本的加減乘除運算。

實作的思路大致上是：
利用一個TIMER，並且設定好進入中斷的週期，而中斷函式內要做的事有兩個：計算轉速、以PID計算輸出值。
一般來說PID計算的週期越短，我們可以更快的達到目標值，但是當週期越短，我們測量馬達轉速的精準度也會下降。因此要在兩者間取個平衡，目前我使用約1ms~10ms這個區間的取樣週期。

程式的結構大致如下

void Motor::pid() {
    /***Parameter***/
	double P = 5000;
	double I = 5;
	double D = 350;
	
	/***Measurement***/
	rps = (double) v_angular / 360 * 1000 / t_measure;

	/*** Control Speed ***/
	err = goal-rps;
	rps_aft = rps;
	proportion = (double) P * err;
	integral += (double) I * err * t_measure / 1000;
	differential = (double) -1 * D * (double) (rps_aft - rps_bef) / t_measure* 1000;
	cycle += proportion + integral + differential;
    
    /***Output***/
}

先計算出馬達的轉速，詳細的使用方式可以看之前的encoder介紹與實際使用教學喔~總之這個步驟就是要給出rps的值就對了。接著第二步驟就是要進行PID的運算，誤差err等於目標值扣掉當前轉速，且根據上面離散型PID的公式，分別計算出PID三個的值，再加總。