2nd Library
Diff: PI_Cntrl.cpp
- Revision:
- 0:a201a6cd4c0c
diff -r 000000000000 -r a201a6cd4c0c PI_Cntrl.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PI_Cntrl.cpp Thu Mar 07 07:03:56 2019 +0000 @@ -0,0 +1,61 @@ +/* + 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 + + Tn*s + 1 + G(s) = Kp ------------- with s ~ (1 - z^-1)/Ts + Ts*s +*/ + +#include "PI_Cntrl.h" + +using namespace std; + +PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts) +{ + setCoefficients(Kp, Tn, Ts); + uMax = 10000000000.0; + uMin = -uMax; + reset(0.0f); +} + +PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax) +{ + setCoefficients(Kp, Tn, Ts); + this->uMax = (double)uMax; + uMin = -(double)uMax; + reset(0.0f); +} + +PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax, float uMin) +{ + setCoefficients(Kp, Tn, Ts); + this->uMax = (double)uMax; + this->uMin = (double)uMin; + reset(0.0f); +} + +PI_Cntrl::~PI_Cntrl() {} + +void PI_Cntrl::reset(float initValue) +{ + s = (double)initValue; +} + +void PI_Cntrl::setCoefficients(float Kp, float Tn, float Ts) +{ + b0 = (double)Kp*(1.0 + (double)Ts/(double)Tn); + b1 = -(double)Kp; + b2 = (double)Ts/(double)Tn; +} + +float PI_Cntrl::doStep(double e) +{ + double u = b0*e + s; // unconstrained output + double uc = u; // constrained output + if(u > uMax) uc = uMax; + else if(u < uMin) uc = uMin; + s = b1*e + u - b2*(u - uc); + return (float)uc; +}