2nd Library
PIDT2_Cntrl.cpp@4:db615f5fa407, 2021-04-27 (annotated)
- Committer:
- altb2
- Date:
- Tue Apr 27 07:49:53 2021 +0000
- Revision:
- 4:db615f5fa407
- Parent:
- 3:e3b84ea63c0f
.
Who changed what in which revision?
User | Revision | Line number | New 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 | } |