2nd Library
Dependents: cuboid_balance_ros cuboid_balance mirror_actuator_V1
PI_Cntrl.cpp
- Committer:
- altb2
- Date:
- 2021-04-01
- Revision:
- 3:e3b84ea63c0f
- Parent:
- 0:a201a6cd4c0c
File content as of revision 3:e3b84ea63c0f:
/* 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; }