Diff: Library_Cntrl/PIDT2_Cntrl.cpp

Revision:

16:28b6bb8a4b7f

Parent:

15:9f32f64eee5b

--- a/Library_Cntrl/PIDT2_Cntrl.cpp	Sun May 02 08:17:06 2021 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,172 +0,0 @@
-/*
-                        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;
-}