Control Library by altb
Dependents: My_Libraries IndNav_QK3_T265
Diff: PID_Cntrl.cpp
- Revision:
- 2:e7c9e50a2e46
- Parent:
- 0:d49418189c5c
- Child:
- 3:27595dd9e433
--- a/PID_Cntrl.cpp Mon Mar 04 11:26:47 2019 +0000 +++ b/PID_Cntrl.cpp Fri Jun 14 07:05:55 2019 +0000 @@ -1,11 +1,11 @@ /* PI Controller class with anti windup reset in biquad transposed direct form 2 see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html - everything is calculated in double + everything is calculated in float - Ts z - 1 - G(s) = P + I ------- + D ------- D corresponds Kd/Tf in Matlab-formlism pid(...) - z - 1 z - p + Ts z - 1 + G(s) = P + I ------- + D/tau ------- D corresponds Kd in Matlab-formlism pid(...) + z - 1 z - p */ #include "PID_Cntrl.h" @@ -14,8 +14,8 @@ PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax) { setCoefficients(P, I, D, tau_f, Ts); - this->uMin = (double)uMin; - this->uMax = (double)uMax; + this->uMin = uMin; + this->uMax = uMax; reset(0.0f); } @@ -23,17 +23,17 @@ void PID_Cntrl::reset(float initValue) { - Iold = (double)initValue; + Iold = initValue; eold = 0.0;yold = 0.0; del = 0.0; } void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float Ts) { - this->p = 1.0 - (double)Ts/(double)tau_f; + this->p = 1.0 - Ts/tau_f; this->P = P; this->I = I; - this->D = D; + this->D_ = D/tau_f; // modified D, now D is consistent with Matlab PID this->Ts = Ts; if(P!=0) this->Ka=1/P; @@ -42,28 +42,28 @@ } -float PID_Cntrl::doStep(double e) +float PID_Cntrl::doStep(float e) { - double Ipart = Iold+I*Ts*(e-del); - double Dpart = D*(e-eold)+p*yold; - double u = P*e + Dpart + Ipart; // unconstrained output - double uc = u; // constrained output + float Ipart = Iold+I*Ts*(e-del); + float Dpart = D_*(e-eold)+p*yold; + 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; yold=Dpart; - return (float)uc; + return uc; } -void PID_Cntrl::set_limits(double ll, double ul) +void PID_Cntrl::set_limits(float ll, float ul) { - this->uMin = (double)ll; - this->uMax = (double)ul; + this->uMin = ll; + this->uMax = ul; } float PID_Cntrl::get_ulimit(void) { - return (float)this->uMax; + return this->uMax; } \ No newline at end of file