Control Library by altb
Dependents: My_Libraries IndNav_QK3_T265
Diff: PID_Cntrl.cpp
- Revision:
- 10:447f6a864598
- Parent:
- 5:d8c53cece01b
- Child:
- 11:7ea3efaf0469
--- a/PID_Cntrl.cpp Fri Sep 20 09:26:23 2019 +0000 +++ b/PID_Cntrl.cpp Mon Sep 23 08:59:29 2019 +0000 @@ -24,13 +24,13 @@ void PID_Cntrl::reset(float initValue) { Iold = initValue; - eold = 0.0;yold = 0.0; + eold = 0.0;yold = 0.0;Dpart_old = 0.0; del = 0.0; } void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float Ts) { - this->p = 1.0 - Ts/tau_f; + this->p = 1.0f - Ts/tau_f; this->P = P; this->P_init = P; this->I = I; @@ -67,7 +67,7 @@ float PID_Cntrl::doStep(float e) { float Ipart = Iold+I*Ts*(e-del); - float Dpart = D_*(e-eold)+p*yold; + float Dpart = D_*(e-eold)+p*Dpart_old; float u = P*e + Dpart + Ipart; // unconstrained output float uc = u; // constrained output if(u > uMax) uc = uMax; @@ -75,7 +75,22 @@ del=(u-uc)*Ka; eold=e; Iold=Ipart; - yold=Dpart; + Dpart_old=Dpart; + return uc; +} +float PID_Cntrl::doStep(float e,float y) +{ + float Ipart = Iold+I*Ts*(e-del); + float Dpart = D_ * (y-yold) + p * Dpart_old; + float u = P*e - Dpart + Ipart; // unconstrained output + float uc = u; // constrained output + if(u > uMax) uc = uMax; + else if(u < uMin) uc = uMin; + del=(u-uc)*Ka; + eold=e; + Iold=Ipart; + Dpart_old = Dpart; + yold = y; return uc; }