enhanced functionality in V01 vs. V00, V02 finished, conversion to double precsision in V03

Dependencies:   mbed

Committer:
pmic
Date:
Mon Apr 09 17:50:45 2018 +0000
Revision:
22:c895fa4d7319
Parent:
12:e54941459353
all in float!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 12:e54941459353 1 /*
pmic 12:e54941459353 2 PI Controller class with anti windup reset in biquad transposed direct form 2
pmic 12:e54941459353 3 see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html
pmic 12:e54941459353 4 everything is calculated in double
pmic 12:e54941459353 5
pmic 12:e54941459353 6 Tn*s + 1
pmic 12:e54941459353 7 G(s) = Kp ------------- with s ~ (1 - z^-1)/Ts
pmic 12:e54941459353 8 Ts*s
pmic 12:e54941459353 9 */
pmic 12:e54941459353 10
rtlabor 0:15be70d21d7c 11 #include "PI_Cntrl.h"
rtlabor 0:15be70d21d7c 12
rtlabor 0:15be70d21d7c 13 using namespace std;
rtlabor 0:15be70d21d7c 14
pmic 12:e54941459353 15 PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts)
pmic 12:e54941459353 16 {
pmic 12:e54941459353 17 setCoefficients(Kp, Tn, Ts);
pmic 12:e54941459353 18 uMax = 10000000000.0;
pmic 12:e54941459353 19 uMin = -uMax;
pmic 12:e54941459353 20 reset(0.0f);
pmic 12:e54941459353 21 }
rtlabor 0:15be70d21d7c 22
pmic 12:e54941459353 23 PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax)
pmic 12:e54941459353 24 {
pmic 12:e54941459353 25 setCoefficients(Kp, Tn, Ts);
pmic 12:e54941459353 26 this->uMax = (double)uMax;
pmic 12:e54941459353 27 uMin = -(double)uMax;
pmic 12:e54941459353 28 reset(0.0f);
pmic 12:e54941459353 29 }
rtlabor 0:15be70d21d7c 30
pmic 12:e54941459353 31 PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax, float uMin)
pmic 12:e54941459353 32 {
pmic 12:e54941459353 33 setCoefficients(Kp, Tn, Ts);
pmic 12:e54941459353 34 this->uMax = (double)uMax;
pmic 12:e54941459353 35 this->uMin = (double)uMin;
pmic 12:e54941459353 36 reset(0.0f);
rtlabor 0:15be70d21d7c 37 }
rtlabor 0:15be70d21d7c 38
pmic 12:e54941459353 39 PI_Cntrl::~PI_Cntrl() {}
pmic 12:e54941459353 40
pmic 12:e54941459353 41 void PI_Cntrl::reset(float initValue)
pmic 12:e54941459353 42 {
pmic 12:e54941459353 43 s = (double)initValue;
pmic 12:e54941459353 44 }
pmic 12:e54941459353 45
pmic 12:e54941459353 46 void PI_Cntrl::setCoefficients(float Kp, float Tn, float Ts)
pmic 12:e54941459353 47 {
pmic 12:e54941459353 48 b0 = (double)Kp*(1.0 + (double)Ts/(double)Tn);
pmic 12:e54941459353 49 b1 = -(double)Kp;
pmic 12:e54941459353 50 b2 = (double)Ts/(double)Tn;
pmic 12:e54941459353 51 }
pmic 12:e54941459353 52
pmic 12:e54941459353 53 float PI_Cntrl::doStep(double e)
pmic 12:e54941459353 54 {
pmic 12:e54941459353 55 double u = b0*e + s; // unconstrained output
pmic 12:e54941459353 56 double uc = u; // constrained output
pmic 12:e54941459353 57 if(u > uMax) uc = uMax;
pmic 12:e54941459353 58 else if(u < uMin) uc = uMin;
pmic 12:e54941459353 59 s = b1*e + u - b2*(u - uc);
pmic 12:e54941459353 60 return (float)uc;
pmic 12:e54941459353 61 }