RT2_Cuboid_demo / Mbed 2 deprecated nucf446-test_GPA

Test of pmic GPA with filter

Dependencies:   mbed

Fork of nucf446-cuboid-balance1_strong by RT2_Cuboid_demo

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?

UserRevisionLine numberNew 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 }