2nd Library
PIDT2_Cntrl.cpp
- Committer:
- altb2
- Date:
- 2021-04-27
- Revision:
- 4:db615f5fa407
- Parent:
- 3:e3b84ea63c0f
File content as of revision 4:db615f5fa407:
/* 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; }