Test of pmic GPA with filter
Dependencies: mbed
Fork of nucf446-cuboid-balance1_strong by
PI_Cntrl.cpp@15:1e8e90b4e3a1, 2018-04-04 (annotated)
- Committer:
- pmic
- Date:
- Wed Apr 04 13:26:23 2018 +0000
- Revision:
- 15:1e8e90b4e3a1
- Parent:
- 14:fa8706c9c984
convert PI_Cntrl class to double prescision;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pmic | 8:d68e177e2571 | 1 | /* |
pmic | 12:93fd84766578 | 2 | PI Controller class with anti windup reset in biquad transposed direct form 2 |
pmic | 12:93fd84766578 | 3 | see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html |
pmic | 15:1e8e90b4e3a1 | 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 | 12:93fd84766578 | 15 | PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts) |
pmic | 8:d68e177e2571 | 16 | { |
pmic | 15:1e8e90b4e3a1 | 17 | setCoefficients(Kp, Tn, Ts); |
pmic | 15:1e8e90b4e3a1 | 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 | 12:93fd84766578 | 23 | PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax) |
pmic | 8:d68e177e2571 | 24 | { |
pmic | 15:1e8e90b4e3a1 | 25 | setCoefficients(Kp, Tn, Ts); |
pmic | 15:1e8e90b4e3a1 | 26 | this->uMax = (double)uMax; |
pmic | 15:1e8e90b4e3a1 | 27 | uMin = -(double)uMax; |
pmic | 8:d68e177e2571 | 28 | reset(0.0f); |
pmic | 8:d68e177e2571 | 29 | } |
rtlabor | 0:15be70d21d7c | 30 | |
pmic | 12:93fd84766578 | 31 | PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax, float uMin) |
pmic | 8:d68e177e2571 | 32 | { |
pmic | 15:1e8e90b4e3a1 | 33 | setCoefficients(Kp, Tn, Ts); |
pmic | 15:1e8e90b4e3a1 | 34 | this->uMax = (double)uMax; |
pmic | 15:1e8e90b4e3a1 | 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 | 15:1e8e90b4e3a1 | 43 | s = (double)initValue; |
pmic | 8:d68e177e2571 | 44 | } |
pmic | 8:d68e177e2571 | 45 | |
pmic | 15:1e8e90b4e3a1 | 46 | void PI_Cntrl::setCoefficients(float Kp, float Tn, float Ts) |
pmic | 8:d68e177e2571 | 47 | { |
pmic | 15:1e8e90b4e3a1 | 48 | b0 = (double)Kp*(1.0 + (double)Ts/(double)Tn); |
pmic | 15:1e8e90b4e3a1 | 49 | b1 = -(double)Kp; |
pmic | 15:1e8e90b4e3a1 | 50 | b2 = (double)Ts/(double)Tn; |
pmic | 15:1e8e90b4e3a1 | 51 | } |
pmic | 15:1e8e90b4e3a1 | 52 | |
pmic | 15:1e8e90b4e3a1 | 53 | float PI_Cntrl::doStep(double e) |
pmic | 15:1e8e90b4e3a1 | 54 | { |
pmic | 15:1e8e90b4e3a1 | 55 | double u = b0*e + s; // unconstrained output |
pmic | 15:1e8e90b4e3a1 | 56 | double uc = u; // constrained output |
pmic | 13:186867e79092 | 57 | if(u > uMax) uc = uMax; |
pmic | 13:186867e79092 | 58 | else if(u < uMin) uc = uMin; |
pmic | 13:186867e79092 | 59 | s = b1*e + u - b2*(u - uc); |
pmic | 15:1e8e90b4e3a1 | 60 | return (float)uc; |
pmic | 8:d68e177e2571 | 61 | } |