Ruprecht Altenburger / Library_Cntrl

2nd Library

Dependents:   cuboid_balance_ros cuboid_balance mirror_actuator_V1

PIDT2_Cntrl.cpp@3:e3b84ea63c0f, 2021-04-01 (annotated)

Committer:
altb2
Date:
Thu Apr 01 13:38:43 2021 +0000
Revision:
3:e3b84ea63c0f
PIDT2 added

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 3:e3b84ea63c0f 1 /*
altb2 3:e3b84ea63c0f 2 Ts
altb2 3:e3b84ea63c0f 3 C(z) = (P + I ---------- + D * tustin(s / (tau_f * s + 1))) * tustin(1 / (tau_ro * s + 1))
altb2 3:e3b84ea63c0f 4 1 - z^-1
altb2 3:e3b84ea63c0f 5
altb2 3:e3b84ea63c0f 6 - Corresponds to MATLAB command: C = pid(P, I, D, tau_f, Ts, 'IFormula', 'BackwardEuler', 'DFormula', 'Trapezoidal') * c2d(tf(1, [tau_rl 1]), Ts, 'tustin')
altb2 3:e3b84ea63c0f 7 - Anti-Windup: Saturation of Ipart and u = P*e + IPart + DPart
altb2 3:e3b84ea63c0f 8 */
altb2 3:e3b84ea63c0f 9
altb2 3:e3b84ea63c0f 10 #include "PIDT2_Cntrl.h"
altb2 3:e3b84ea63c0f 11
altb2 3:e3b84ea63c0f 12 PIDT2_Cntrl::PIDT2_Cntrl(float P, float I, float D, float tau_f, float tau_ro, float Ts, float uMin, float uMax) {
altb2 3:e3b84ea63c0f 13 setCoefficients(P, I, D, tau_f, tau_ro, Ts);
altb2 3:e3b84ea63c0f 14 this -> uMin = uMin;
altb2 3:e3b84ea63c0f 15 this -> uMax = uMax;
altb2 3:e3b84ea63c0f 16 reset(0.0f);
altb2 3:e3b84ea63c0f 17 }
altb2 3:e3b84ea63c0f 18
altb2 3:e3b84ea63c0f 19 PIDT2_Cntrl::~PIDT2_Cntrl() {}
altb2 3:e3b84ea63c0f 20
altb2 3:e3b84ea63c0f 21 void PIDT2_Cntrl::reset(float initValue) {
altb2 3:e3b84ea63c0f 22 IPart = initValue;
altb2 3:e3b84ea63c0f 23 Dpart = 0.0f;
altb2 3:e3b84ea63c0f 24 d_old = 0.0f;
altb2 3:e3b84ea63c0f 25 u_old = initValue;
altb2 3:e3b84ea63c0f 26 uf = initValue;
altb2 3:e3b84ea63c0f 27 }
altb2 3:e3b84ea63c0f 28
altb2 3:e3b84ea63c0f 29 void PIDT2_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float tau_ro, float Ts, float uMin, float uMax) {
altb2 3:e3b84ea63c0f 30 setCoefficients(P, I, D, tau_f, tau_ro, Ts);
altb2 3:e3b84ea63c0f 31 this -> uMin = uMin;
altb2 3:e3b84ea63c0f 32 this -> uMax = uMax;
altb2 3:e3b84ea63c0f 33 reset(0.0f);
altb2 3:e3b84ea63c0f 34 }
altb2 3:e3b84ea63c0f 35
altb2 3:e3b84ea63c0f 36 void PIDT2_Cntrl::setCoeff_P(float P) {
altb2 3:e3b84ea63c0f 37 this -> P = P;
altb2 3:e3b84ea63c0f 38 }
altb2 3:e3b84ea63c0f 39 void PIDT2_Cntrl::setCoeff_I(float I) {
altb2 3:e3b84ea63c0f 40 this -> I = I;
altb2 3:e3b84ea63c0f 41 this -> bi = I * Ts;
altb2 3:e3b84ea63c0f 42 }
altb2 3:e3b84ea63c0f 43 void PIDT2_Cntrl::setCoeff_D(float D) {
altb2 3:e3b84ea63c0f 44 this -> D = D;
altb2 3:e3b84ea63c0f 45 this -> bd = 2.0f * D / (Ts + 2.0f * tau_f);
altb2 3:e3b84ea63c0f 46 }
altb2 3:e3b84ea63c0f 47 void PIDT2_Cntrl::scale_PIDT2_param(float scale) {
altb2 3:e3b84ea63c0f 48 this -> P = P_init * scale;
altb2 3:e3b84ea63c0f 49 this -> I = I_init * scale;
altb2 3:e3b84ea63c0f 50 this -> D = D_init * scale;
altb2 3:e3b84ea63c0f 51 this -> bi = I_init * Ts * scale;
altb2 3:e3b84ea63c0f 52 this -> bd = 2.0f * D_init / (Ts + 2.0f * tau_f) * scale;
altb2 3:e3b84ea63c0f 53 }
altb2 3:e3b84ea63c0f 54
altb2 3:e3b84ea63c0f 55 float PIDT2_Cntrl::update(float e) {
altb2 3:e3b84ea63c0f 56 IPart = saturate(IPart + bi * e, uMin, uMax);
altb2 3:e3b84ea63c0f 57 Dpart = bd * (e - d_old) - ad * Dpart;
altb2 3:e3b84ea63c0f 58 d_old = e;
altb2 3:e3b84ea63c0f 59 float u = P * e + IPart + Dpart;
altb2 3:e3b84ea63c0f 60 uf = saturate(bf * (u + u_old) - af * uf, uMin, uMax);
altb2 3:e3b84ea63c0f 61 u_old = u;
altb2 3:e3b84ea63c0f 62 return uf;
altb2 3:e3b84ea63c0f 63 }
altb2 3:e3b84ea63c0f 64
altb2 3:e3b84ea63c0f 65 float PIDT2_Cntrl::update(float e, float y) {
altb2 3:e3b84ea63c0f 66
altb2 3:e3b84ea63c0f 67 IPart = saturate(IPart + bi * e, uMin, uMax);
altb2 3:e3b84ea63c0f 68 Dpart = bd * (y - d_old) - ad * Dpart;
altb2 3:e3b84ea63c0f 69
altb2 3:e3b84ea63c0f 70 d_old = y;
altb2 3:e3b84ea63c0f 71 float u = P * e + IPart - Dpart;
altb2 3:e3b84ea63c0f 72
altb2 3:e3b84ea63c0f 73 uf = saturate(bf * (u + u_old) - af * uf, uMin, uMax);
altb2 3:e3b84ea63c0f 74 u_old = u;
altb2 3:e3b84ea63c0f 75
altb2 3:e3b84ea63c0f 76 return uf;
altb2 3:e3b84ea63c0f 77 }
altb2 3:e3b84ea63c0f 78
altb2 3:e3b84ea63c0f 79 void PIDT2_Cntrl::set_limits(float uMin, float uMax) {
altb2 3:e3b84ea63c0f 80 this -> uMin = uMin;
altb2 3:e3b84ea63c0f 81 this -> uMax = uMax;
altb2 3:e3b84ea63c0f 82 }
altb2 3:e3b84ea63c0f 83
altb2 3:e3b84ea63c0f 84 float PIDT2_Cntrl::get_ulimit() {
altb2 3:e3b84ea63c0f 85 return this -> uMax;
altb2 3:e3b84ea63c0f 86 }
altb2 3:e3b84ea63c0f 87
altb2 3:e3b84ea63c0f 88 float PIDT2_Cntrl::get_P_gain() {
altb2 3:e3b84ea63c0f 89 return this -> P;
altb2 3:e3b84ea63c0f 90 }
altb2 3:e3b84ea63c0f 91
altb2 3:e3b84ea63c0f 92 float PIDT2_Cntrl::get_I() {
altb2 3:e3b84ea63c0f 93 return this -> I;
altb2 3:e3b84ea63c0f 94 }
altb2 3:e3b84ea63c0f 95
altb2 3:e3b84ea63c0f 96 float PIDT2_Cntrl::get_D() {
altb2 3:e3b84ea63c0f 97 return this -> D;
altb2 3:e3b84ea63c0f 98 }
altb2 3:e3b84ea63c0f 99
altb2 3:e3b84ea63c0f 100 float PIDT2_Cntrl::get_bd() {
altb2 3:e3b84ea63c0f 101 return this -> bd;
altb2 3:e3b84ea63c0f 102 }
altb2 3:e3b84ea63c0f 103
altb2 3:e3b84ea63c0f 104 float PIDT2_Cntrl::get_ad() {
altb2 3:e3b84ea63c0f 105 return this -> ad;
altb2 3:e3b84ea63c0f 106 }
altb2 3:e3b84ea63c0f 107
altb2 3:e3b84ea63c0f 108 float PIDT2_Cntrl::get_bi() {
altb2 3:e3b84ea63c0f 109 return this -> bi;
altb2 3:e3b84ea63c0f 110 }
altb2 3:e3b84ea63c0f 111
altb2 3:e3b84ea63c0f 112 float PIDT2_Cntrl::get_bf() {
altb2 3:e3b84ea63c0f 113 return this -> bf;
altb2 3:e3b84ea63c0f 114 }
altb2 3:e3b84ea63c0f 115
altb2 3:e3b84ea63c0f 116 float PIDT2_Cntrl::get_af() {
altb2 3:e3b84ea63c0f 117 return this -> bf;
altb2 3:e3b84ea63c0f 118 }
altb2 3:e3b84ea63c0f 119
altb2 3:e3b84ea63c0f 120 void PIDT2_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float tau_ro, float Ts) {
altb2 3:e3b84ea63c0f 121 /* store parameters */
altb2 3:e3b84ea63c0f 122 this -> P = P;
altb2 3:e3b84ea63c0f 123 this -> I = I;
altb2 3:e3b84ea63c0f 124 this -> D = D;
altb2 3:e3b84ea63c0f 125 this -> tau_f = tau_f;
altb2 3:e3b84ea63c0f 126 this -> tau_ro = tau_ro;
altb2 3:e3b84ea63c0f 127
altb2 3:e3b84ea63c0f 128 double P_d = static_cast < double > (P);
altb2 3:e3b84ea63c0f 129 double I_d = static_cast < double > (I);
altb2 3:e3b84ea63c0f 130 double D_d = static_cast < double > (D);
altb2 3:e3b84ea63c0f 131
altb2 3:e3b84ea63c0f 132 // double tau_f_d = static_cast < double > (tau_f);
altb2 3:e3b84ea63c0f 133 // double tau_ro_d = static_cast < double > (tau_ro);
altb2 3:e3b84ea63c0f 134
altb2 3:e3b84ea63c0f 135 // double Ts_d = static_cast < double > (Ts);
altb2 3:e3b84ea63c0f 136
altb2 3:e3b84ea63c0f 137 // double bi_d = I_d * Ts_d;
altb2 3:e3b84ea63c0f 138 // this -> bi = static_cast < float > (bi_d);
altb2 3:e3b84ea63c0f 139
altb2 3:e3b84ea63c0f 140 // double bd_d = 2.0 * D_d / (Ts_d + (2.0 * tau_f_d));
altb2 3:e3b84ea63c0f 141 // this -> bd = static_cast < float > (bd_d);
altb2 3:e3b84ea63c0f 142
altb2 3:e3b84ea63c0f 143 // double ad_d = (Ts_d - 2.0 * tau_f_d) / (Ts_d + 2.0 * tau_f_d);
altb2 3:e3b84ea63c0f 144 // this -> ad = static_cast < float > (ad_d);
altb2 3:e3b84ea63c0f 145
altb2 3:e3b84ea63c0f 146 // double bf_d = Ts_d / (Ts_d + 2.0 * tau_ro_d);
altb2 3:e3b84ea63c0f 147 // this -> bf = static_cast < float > (bf_d);
altb2 3:e3b84ea63c0f 148
altb2 3:e3b84ea63c0f 149 // double af_d = (Ts_d - 2.0 * tau_ro_d) / (Ts_d + 2.0 * tau_ro_d);
altb2 3:e3b84ea63c0f 150 // this -> af = static_cast < float > (af_d);
altb2 3:e3b84ea63c0f 151
altb2 3:e3b84ea63c0f 152 this -> bi = I * Ts;
altb2 3:e3b84ea63c0f 153 this -> bd = 2.0f * D / (Ts + (2.0f * tau_f));
altb2 3:e3b84ea63c0f 154 this -> ad = (Ts - 2.0f * tau_f) / (Ts + 2.0f * tau_f);
altb2 3:e3b84ea63c0f 155 this -> bf = Ts / (Ts + 2.0f * tau_ro);
altb2 3:e3b84ea63c0f 156 this -> af = (Ts - 2.0f * tau_ro) / (Ts + 2.0f * tau_ro);
altb2 3:e3b84ea63c0f 157
altb2 3:e3b84ea63c0f 158
altb2 3:e3b84ea63c0f 159 /* store initial PIDT2-algorithm parameters */
altb2 3:e3b84ea63c0f 160 this -> P_init = P;
altb2 3:e3b84ea63c0f 161 this -> I_init = I;
altb2 3:e3b84ea63c0f 162 this -> D_init = D;
altb2 3:e3b84ea63c0f 163 }
altb2 3:e3b84ea63c0f 164
altb2 3:e3b84ea63c0f 165 float PIDT2_Cntrl::saturate(float u, float uMin, float uMax) {
altb2 3:e3b84ea63c0f 166 if (u > uMax) {
altb2 3:e3b84ea63c0f 167 u = uMax;
altb2 3:e3b84ea63c0f 168 } else if (u < uMin) {
altb2 3:e3b84ea63c0f 169 u = uMin;
altb2 3:e3b84ea63c0f 170 }
altb2 3:e3b84ea63c0f 171 return u;
altb2 3:e3b84ea63c0f 172 }