Test of pmic GPA with filter
Dependencies: mbed
Fork of nucf446-cuboid-balance1_strong by
PI_Cntrl.cpp
- Committer:
- pmic
- Date:
- 2018-04-04
- Revision:
- 14:fa8706c9c984
- Parent:
- 13:186867e79092
- Child:
- 15:1e8e90b4e3a1
File content as of revision 14:fa8706c9c984:
/* 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 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) { b0 = Kp*(1.0f + Ts/Tn); b1 = -Kp; b2 = Ts/Tn; uMax = 10000000000.0f; uMin = -uMax; reset(0.0f); } PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax) { b0 = Kp*(1.0f + Ts/Tn); b1 = -Kp; b2 = Ts/Tn; this->uMax = uMax; uMin = -uMax; reset(0.0f); } PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax, float uMin) { b0 = Kp*(1.0f + Ts/Tn); b1 = -Kp; b2 = Ts/Tn; this->uMax = uMax; this->uMin = uMin; reset(0.0f); } PI_Cntrl::~PI_Cntrl() {} void PI_Cntrl::reset(float initValue) { s = initValue; } float PI_Cntrl::doStep(float e) { float u = b0*e + s; // unconstrained output float uc = u; // constrained output if(u > uMax) uc = uMax; else if(u < uMin) uc = uMin; s = b1*e + u - b2*(u - uc); return uc; }