Test program with the RT black boxes

Dependencies:   mbed

Committer:
pmic
Date:
Wed May 15 13:58:10 2019 +0000
Revision:
43:a577f752dd7a
Parent:
10:a28f393c6716
Add new instantiation option to surpress the printout for rt mbed

Who changed what in which revision?

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