2nd Library
Diff: PID_Cntrl.cpp
- Revision:
- 0:a201a6cd4c0c
diff -r 000000000000 -r a201a6cd4c0c PID_Cntrl.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID_Cntrl.cpp Thu Mar 07 07:03:56 2019 +0000 @@ -0,0 +1,69 @@ +/* + 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 + + Ts z - 1 + G(s) = P + I ------- + D ------- D corresponds Kd/Tf in Matlab-formlism pid(...) + z - 1 z - p +*/ + +#include "PID_Cntrl.h" +using namespace std; + +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; + reset(0.0f); +} + +PID_Cntrl::~PID_Cntrl() {} + +void PID_Cntrl::reset(float initValue) +{ + Iold = (double)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 = P; + this->I = I; + this->D = D; + this->Ts = Ts; + if(P!=0) + this->Ka=1/P; + else + this->Ka=1.0f; + +} + +float PID_Cntrl::doStep(double 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 + if(u > uMax) uc = uMax; + else if(u < uMin) uc = uMin; + del=(u-uc)*Ka; + eold=e; + Iold=Ipart; + yold=Dpart; + return (float)uc; +} + +void PID_Cntrl::set_limits(double ll, double ul) +{ + this->uMin = (double)ll; + this->uMax = (double)ul; +} + +float PID_Cntrl::get_ulimit(void) +{ + return (float)this->uMax; +}