當(dāng)然 為了省事 我們當(dāng)然去搞增量PID 了 何況 前輩們都說(shuō)這個(gè)比較好
PID 計(jì)算資源需求
DS=Pdu(k)+I t/ti( uk)+ D TD/T duk+du(k-1)
puk-i *T/TI*uk+d*(uk-2* uk-1 + uk-2)
三個(gè)系統(tǒng)狀態(tài)是需要的 UK UK-1 UK-2
還有P I D 的參數(shù)也是需要的
當(dāng)然還有個(gè) 采樣時(shí)間 T （似乎也有人 把積分時(shí)間 T 和微分時(shí)間T 分開(kāi)了？ 我不是很明白不管他）
應(yīng)為不同的硬件體系 我們需要計(jì)算數(shù)據(jù)的位數(shù) 有差異（可能需要int型 去算 也可能用 long ，floAt 等）
應(yīng)為是C18下的 我先試用INT 去做
#define PID_TYPE int
這樣以后換類型也省事
于是我們定義1個(gè)結(jié)構(gòu)體
struct pid_unit {
PID_TYPE pid_sens[3]; // 用來(lái)保存 UK UK-1 UK-2 三個(gè)時(shí)刻的輸出偏差
PID_TYPE pid_cpid[3]; // 當(dāng)然是 P I D 三個(gè)參數(shù)咯
PID_TYPE pid_dpid[3]; // P 部分計(jì)算值 I部分計(jì)算值 D 部分計(jì)算值 這三個(gè)值相加就是PID 的輸出
};
初始化PID
void pid_init(struct pid_unit *unit,
PID_TYPE p,
PID_TYPE i,
PID_TYPE d)
{
unit->pid_cpid[0] = p; //PID 比例系數(shù) 初始化 沒(méi)得說(shuō) 哈
unit->pid_cpid[1] = i;
unit->pid_cpid[2] = d;
unit->pid_dpid[0] = 0;//P i d 個(gè)個(gè)部分都假定為0 不過(guò)實(shí)際也是0#24
unit->pid_dpid[1] = 0; //
unit->pid_dpid[2] = 0; //
unit->pid_sens[0] = 0; // 當(dāng)前差值
unit->pid_sens[1] = 0; //
unit->pid_sens[2] = 0; //
}
接下來(lái) 就是計(jì)算部分了
第一部分 P部分 計(jì)算
unit->pid_dpid[0] = unit->pid_sens[0] * unit->pid_cpid[0]; //就是P*UK
然后是I部分 。。不過(guò)大家會(huì)發(fā)現(xiàn)I還需要前一次的計(jì)算結(jié)果。。咋辦。。
第一次 用的是0。n那么第二次 用的就是現(xiàn)在的咯
所以要保存現(xiàn)在的值 給下一次用
同時(shí)保存上一次的值 到上上一次
說(shuō)的這么拗口，。那是我語(yǔ)文沒(méi)學(xué)好
其實(shí)就是保存三個(gè)時(shí)間狀態(tài)值
丟棄最老的 保存最新的
unit->pid_sens[1] = unit->pid_sens[0];
unit->pid_sens[0] = input;
unit->pid_dpid[1] = unit->pid_sens[0] * unit->pid_cpid[1] * time;
等我吃個(gè)飯?jiān)賮?lái) 有點(diǎn)問(wèn)題
GOOGLE 源碼是 ：
unit->pid_dpid[1] += unit->pid_sens[0] * unit->pid_cpid[1] * time;
而ΔU = U(k)-U(k-1) = Kp*[e(k)-e(k-1)]+Ki*e(k)+Kd*[e(k)-2*e(k-1)+e(k-2)]
是肯定沒(méi)問(wèn)題的。。所以 似乎GOOGLE 上源碼不對(duì)哦，，大家要注意了
D部分 Kd*[e(k)-2*e(k-1)+e(k-2)
unit->pid_dpid[2] = ((unit->pid_sens[0] - 2*unit->pid_sens[1]+unit->pid_sens[2])/time) *unit->pid_cpid[2];
然后對(duì)P I D 求和
unit->pid_dpid[0] + unit->pid_dpid[1] + unit->pid_dpid[2];
故有PID 計(jì)算子函數(shù)如下
PID_TYPE pid_control(struct pid_unit *unit,
PID_TYPE input,
PID_TYPE time)
{
// adjust the FIFO preserving the sensor data
unit->pid_sens[1] = unit->pid_sens[0];
unit->pid_sens[0] = input;
// calculate each pid variable
unit->pid_dpid[0] = unit->pid_sens[0] * unit->pid_cpid[0];
unit->pid_dpid[1] = unit->pid_sens[0] * unit->pid_cpid[1] * time;
unit->pid_dpid[2] = (unit->pid_sens[0] - unit->pid_sens[1])/time *unit->pid_cpid[2];
return unit->pid_dpid[0] +
unit->pid_dpid[1] +
unit->pid_dpid[2];
}