.
Fork of Cntrlol_Lib by
Embed:
(wiki syntax)
Show/hide line numbers
PI_Cntrl.cpp
00001 /* 00002 PI Controller class with anti windup reset in biquad transposed direct form 2 00003 see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html 00004 everything is calculated in double 00005 00006 Tn*s + 1 00007 G(s) = Kp ------------- with s ~ (1 - z^-1)/Ts 00008 Ts*s 00009 */ 00010 00011 #include "PI_Cntrl.h" 00012 00013 using namespace std; 00014 00015 PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts) 00016 { 00017 setCoefficients(Kp, Tn, Ts); 00018 uMax = 10000000000.0; 00019 uMin = -uMax; 00020 reset(0.0f); 00021 } 00022 00023 PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax) 00024 { 00025 setCoefficients(Kp, Tn, Ts); 00026 this->uMax = (double)uMax; 00027 uMin = -(double)uMax; 00028 reset(0.0f); 00029 } 00030 00031 PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax, float uMin) 00032 { 00033 setCoefficients(Kp, Tn, Ts); 00034 this->uMax = (double)uMax; 00035 this->uMin = (double)uMin; 00036 reset(0.0f); 00037 } 00038 00039 PI_Cntrl::~PI_Cntrl() {} 00040 00041 void PI_Cntrl::reset(float initValue) 00042 { 00043 s = (double)initValue; 00044 } 00045 00046 void PI_Cntrl::setCoefficients(float Kp, float Tn, float Ts) 00047 { 00048 b0 = (double)Kp*(1.0 + (double)Ts/(double)Tn); 00049 b1 = -(double)Kp; 00050 b2 = (double)Ts/(double)Tn; 00051 } 00052 00053 float PI_Cntrl::doStep(double e) 00054 { 00055 double u = b0*e + s; // unconstrained output 00056 double uc = u; // constrained output 00057 if(u > uMax) uc = uMax; 00058 else if(u < uMin) uc = uMin; 00059 s = b1*e + u - b2*(u - uc); 00060 return (float)uc; 00061 }
Generated on Wed Jul 20 2022 13:09:12 by 1.7.2