2nd Library
Embed:
(wiki syntax)
Show/hide line numbers
PIDT2_Cntrl.cpp
00001 /* 00002 Ts 00003 C(z) = (P + I ---------- + D * tustin(s / (tau_f * s + 1))) * tustin(1 / (tau_ro * s + 1)) 00004 1 - z^-1 00005 00006 - Corresponds to MATLAB command: C = pid(P, I, D, tau_f, Ts, 'IFormula', 'BackwardEuler', 'DFormula', 'Trapezoidal') * c2d(tf(1, [tau_rl 1]), Ts, 'tustin') 00007 - Anti-Windup: Saturation of Ipart and u = P*e + IPart + DPart 00008 */ 00009 00010 #include "PIDT2_Cntrl.h" 00011 00012 PIDT2_Cntrl::PIDT2_Cntrl(float P, float I, float D, float tau_f, float tau_ro, float Ts, float uMin, float uMax) { 00013 setCoefficients(P, I, D, tau_f, tau_ro, Ts); 00014 this -> uMin = uMin; 00015 this -> uMax = uMax; 00016 reset(0.0f); 00017 } 00018 00019 PIDT2_Cntrl::~PIDT2_Cntrl() {} 00020 00021 void PIDT2_Cntrl::reset(float initValue) { 00022 IPart = initValue; 00023 Dpart = 0.0f; 00024 d_old = 0.0f; 00025 u_old = initValue; 00026 uf = initValue; 00027 } 00028 00029 void PIDT2_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float tau_ro, float Ts, float uMin, float uMax) { 00030 setCoefficients(P, I, D, tau_f, tau_ro, Ts); 00031 this -> uMin = uMin; 00032 this -> uMax = uMax; 00033 reset(0.0f); 00034 } 00035 00036 void PIDT2_Cntrl::setCoeff_P(float P) { 00037 this -> P = P; 00038 } 00039 void PIDT2_Cntrl::setCoeff_I(float I) { 00040 this -> I = I; 00041 this -> bi = I * Ts; 00042 } 00043 void PIDT2_Cntrl::setCoeff_D(float D) { 00044 this -> D = D; 00045 this -> bd = 2.0f * D / (Ts + 2.0f * tau_f); 00046 } 00047 void PIDT2_Cntrl::scale_PIDT2_param(float scale) { 00048 this -> P = P_init * scale; 00049 this -> I = I_init * scale; 00050 this -> D = D_init * scale; 00051 this -> bi = I_init * Ts * scale; 00052 this -> bd = 2.0f * D_init / (Ts + 2.0f * tau_f) * scale; 00053 } 00054 00055 float PIDT2_Cntrl::update(float e) { 00056 IPart = saturate(IPart + bi * e, uMin, uMax); 00057 Dpart = bd * (e - d_old) - ad * Dpart; 00058 d_old = e; 00059 float u = P * e + IPart + Dpart; 00060 uf = saturate(bf * (u + u_old) - af * uf, uMin, uMax); 00061 u_old = u; 00062 return uf; 00063 } 00064 00065 float PIDT2_Cntrl::update(float e, float y) { 00066 00067 IPart = saturate(IPart + bi * e, uMin, uMax); 00068 Dpart = bd * (y - d_old) - ad * Dpart; 00069 00070 d_old = y; 00071 float u = P * e + IPart - Dpart; 00072 00073 uf = saturate(bf * (u + u_old) - af * uf, uMin, uMax); 00074 u_old = u; 00075 00076 return uf; 00077 } 00078 00079 void PIDT2_Cntrl::set_limits(float uMin, float uMax) { 00080 this -> uMin = uMin; 00081 this -> uMax = uMax; 00082 } 00083 00084 float PIDT2_Cntrl::get_ulimit() { 00085 return this -> uMax; 00086 } 00087 00088 float PIDT2_Cntrl::get_P_gain() { 00089 return this -> P; 00090 } 00091 00092 float PIDT2_Cntrl::get_I() { 00093 return this -> I; 00094 } 00095 00096 float PIDT2_Cntrl::get_D() { 00097 return this -> D; 00098 } 00099 00100 float PIDT2_Cntrl::get_bd() { 00101 return this -> bd; 00102 } 00103 00104 float PIDT2_Cntrl::get_ad() { 00105 return this -> ad; 00106 } 00107 00108 float PIDT2_Cntrl::get_bi() { 00109 return this -> bi; 00110 } 00111 00112 float PIDT2_Cntrl::get_bf() { 00113 return this -> bf; 00114 } 00115 00116 float PIDT2_Cntrl::get_af() { 00117 return this -> bf; 00118 } 00119 00120 void PIDT2_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float tau_ro, float Ts) { 00121 /* store parameters */ 00122 this -> P = P; 00123 this -> I = I; 00124 this -> D = D; 00125 this -> tau_f = tau_f; 00126 this -> tau_ro = tau_ro; 00127 00128 double P_d = static_cast < double > (P); 00129 double I_d = static_cast < double > (I); 00130 double D_d = static_cast < double > (D); 00131 00132 // double tau_f_d = static_cast < double > (tau_f); 00133 // double tau_ro_d = static_cast < double > (tau_ro); 00134 00135 // double Ts_d = static_cast < double > (Ts); 00136 00137 // double bi_d = I_d * Ts_d; 00138 // this -> bi = static_cast < float > (bi_d); 00139 00140 // double bd_d = 2.0 * D_d / (Ts_d + (2.0 * tau_f_d)); 00141 // this -> bd = static_cast < float > (bd_d); 00142 00143 // double ad_d = (Ts_d - 2.0 * tau_f_d) / (Ts_d + 2.0 * tau_f_d); 00144 // this -> ad = static_cast < float > (ad_d); 00145 00146 // double bf_d = Ts_d / (Ts_d + 2.0 * tau_ro_d); 00147 // this -> bf = static_cast < float > (bf_d); 00148 00149 // double af_d = (Ts_d - 2.0 * tau_ro_d) / (Ts_d + 2.0 * tau_ro_d); 00150 // this -> af = static_cast < float > (af_d); 00151 00152 this -> bi = I * Ts; 00153 this -> bd = 2.0f * D / (Ts + (2.0f * tau_f)); 00154 this -> ad = (Ts - 2.0f * tau_f) / (Ts + 2.0f * tau_f); 00155 this -> bf = Ts / (Ts + 2.0f * tau_ro); 00156 this -> af = (Ts - 2.0f * tau_ro) / (Ts + 2.0f * tau_ro); 00157 00158 00159 /* store initial PIDT2-algorithm parameters */ 00160 this -> P_init = P; 00161 this -> I_init = I; 00162 this -> D_init = D; 00163 } 00164 00165 float PIDT2_Cntrl::saturate(float u, float uMin, float uMax) { 00166 if (u > uMax) { 00167 u = uMax; 00168 } else if (u < uMin) { 00169 u = uMin; 00170 } 00171 return u; 00172 }
Generated on Sun Aug 21 2022 12:52:06 by
1.7.2