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 }