![](/media/cache/profiles/altb_JGUSAOG.jpg.50x50_q85.jpg)
Mirror actuator for RT2 lab
Diff: Library_Cntrl/PI_Cntrl.cpp
- Revision:
- 16:28b6bb8a4b7f
- Parent:
- 15:9f32f64eee5b
--- a/Library_Cntrl/PI_Cntrl.cpp Sun May 02 08:17:06 2021 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,61 +0,0 @@ -/* - 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; -}