Digital PI Controller

PI 控制器的基本雛形，看懂用意，就可以進一步修改，套用至自己的控制系統中

1. P 修正當下這次誤差，乘上Kp比例值，決定修正多少
2. I 累積上次的誤差修正量，目的就是希望命令等於迴授

下面是一個 PI 簡單示範

/* proportional term */

f32Error = f32Command - f32Feadback;

/* integral term */

f32Idata += f32Ki * f32Error ;

/* control output */

f32Pdata = f32Kp * f32Error ;

f32Out = f32Pdata + f32Idata;