2nd Library

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers PIDT2_Cntrl.cpp Source File

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 }