2nd Library
Dependents: cuboid_balance_ros cuboid_balance mirror_actuator_V1
Diff: PIDT2_Cntrl.cpp
- Revision:
- 3:e3b84ea63c0f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PIDT2_Cntrl.cpp Thu Apr 01 13:38:43 2021 +0000 @@ -0,0 +1,172 @@ +/* + Ts + C(z) = (P + I ---------- + D * tustin(s / (tau_f * s + 1))) * tustin(1 / (tau_ro * s + 1)) + 1 - z^-1 + + - Corresponds to MATLAB command: C = pid(P, I, D, tau_f, Ts, 'IFormula', 'BackwardEuler', 'DFormula', 'Trapezoidal') * c2d(tf(1, [tau_rl 1]), Ts, 'tustin') + - Anti-Windup: Saturation of Ipart and u = P*e + IPart + DPart +*/ + +#include "PIDT2_Cntrl.h" + +PIDT2_Cntrl::PIDT2_Cntrl(float P, float I, float D, float tau_f, float tau_ro, float Ts, float uMin, float uMax) { + setCoefficients(P, I, D, tau_f, tau_ro, Ts); + this -> uMin = uMin; + this -> uMax = uMax; + reset(0.0f); +} + +PIDT2_Cntrl::~PIDT2_Cntrl() {} + +void PIDT2_Cntrl::reset(float initValue) { + IPart = initValue; + Dpart = 0.0f; + d_old = 0.0f; + u_old = initValue; + uf = initValue; +} + +void PIDT2_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float tau_ro, float Ts, float uMin, float uMax) { + setCoefficients(P, I, D, tau_f, tau_ro, Ts); + this -> uMin = uMin; + this -> uMax = uMax; + reset(0.0f); +} + +void PIDT2_Cntrl::setCoeff_P(float P) { + this -> P = P; +} +void PIDT2_Cntrl::setCoeff_I(float I) { + this -> I = I; + this -> bi = I * Ts; +} +void PIDT2_Cntrl::setCoeff_D(float D) { + this -> D = D; + this -> bd = 2.0f * D / (Ts + 2.0f * tau_f); +} +void PIDT2_Cntrl::scale_PIDT2_param(float scale) { + this -> P = P_init * scale; + this -> I = I_init * scale; + this -> D = D_init * scale; + this -> bi = I_init * Ts * scale; + this -> bd = 2.0f * D_init / (Ts + 2.0f * tau_f) * scale; +} + +float PIDT2_Cntrl::update(float e) { + IPart = saturate(IPart + bi * e, uMin, uMax); + Dpart = bd * (e - d_old) - ad * Dpart; + d_old = e; + float u = P * e + IPart + Dpart; + uf = saturate(bf * (u + u_old) - af * uf, uMin, uMax); + u_old = u; + return uf; +} + +float PIDT2_Cntrl::update(float e, float y) { + + IPart = saturate(IPart + bi * e, uMin, uMax); + Dpart = bd * (y - d_old) - ad * Dpart; + + d_old = y; + float u = P * e + IPart - Dpart; + + uf = saturate(bf * (u + u_old) - af * uf, uMin, uMax); + u_old = u; + + return uf; +} + +void PIDT2_Cntrl::set_limits(float uMin, float uMax) { + this -> uMin = uMin; + this -> uMax = uMax; +} + +float PIDT2_Cntrl::get_ulimit() { + return this -> uMax; +} + +float PIDT2_Cntrl::get_P_gain() { + return this -> P; +} + +float PIDT2_Cntrl::get_I() { + return this -> I; +} + +float PIDT2_Cntrl::get_D() { + return this -> D; +} + +float PIDT2_Cntrl::get_bd() { + return this -> bd; +} + +float PIDT2_Cntrl::get_ad() { + return this -> ad; +} + +float PIDT2_Cntrl::get_bi() { + return this -> bi; +} + +float PIDT2_Cntrl::get_bf() { + return this -> bf; +} + +float PIDT2_Cntrl::get_af() { + return this -> bf; +} + +void PIDT2_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float tau_ro, float Ts) { + /* store parameters */ + this -> P = P; + this -> I = I; + this -> D = D; + this -> tau_f = tau_f; + this -> tau_ro = tau_ro; + + double P_d = static_cast < double > (P); + double I_d = static_cast < double > (I); + double D_d = static_cast < double > (D); + +// double tau_f_d = static_cast < double > (tau_f); +// double tau_ro_d = static_cast < double > (tau_ro); + +// double Ts_d = static_cast < double > (Ts); + +// double bi_d = I_d * Ts_d; +// this -> bi = static_cast < float > (bi_d); + +// double bd_d = 2.0 * D_d / (Ts_d + (2.0 * tau_f_d)); +// this -> bd = static_cast < float > (bd_d); + +// double ad_d = (Ts_d - 2.0 * tau_f_d) / (Ts_d + 2.0 * tau_f_d); +// this -> ad = static_cast < float > (ad_d); + +// double bf_d = Ts_d / (Ts_d + 2.0 * tau_ro_d); +// this -> bf = static_cast < float > (bf_d); + +// double af_d = (Ts_d - 2.0 * tau_ro_d) / (Ts_d + 2.0 * tau_ro_d); +// this -> af = static_cast < float > (af_d); + + this -> bi = I * Ts; + this -> bd = 2.0f * D / (Ts + (2.0f * tau_f)); + this -> ad = (Ts - 2.0f * tau_f) / (Ts + 2.0f * tau_f); + this -> bf = Ts / (Ts + 2.0f * tau_ro); + this -> af = (Ts - 2.0f * tau_ro) / (Ts + 2.0f * tau_ro); + + + /* store initial PIDT2-algorithm parameters */ + this -> P_init = P; + this -> I_init = I; + this -> D_init = D; +} + +float PIDT2_Cntrl::saturate(float u, float uMin, float uMax) { + if (u > uMax) { + u = uMax; + } else if (u < uMin) { + u = uMin; + } + return u; +}