Revision 3:e3b84ea63c0f, committed 2021-04-01

Comitter:

altb2

Date:

Thu Apr 01 13:38:43 2021 +0000

Parent:

2:b54eb3e24d2d

Commit message:

PIDT2 added

Changed in this revision

diff -r b54eb3e24d2d -r e3b84ea63c0f PIDT2_Cntrl.cpp
--- /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;
+}

diff -r b54eb3e24d2d -r e3b84ea63c0f PIDT2_Cntrl.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PIDT2_Cntrl.h	Thu Apr 01 13:38:43 2021 +0000
@@ -0,0 +1,57 @@
+#ifndef PIDT2_CNTRL_H_
+#define PIDT2_CNTRL_H_
+
+#include "mbed.h"
+
+class PIDT2_Cntrl
+{
+public:
+
+    PIDT2_Cntrl(float P, float I, float D, float tau_f, float tau_ro, float Ts, float uMin, float uMax);
+
+    PIDT2_Cntrl() {};
+
+    float operator()(float e)
+    {
+        return update(e);
+    }
+    float operator()(float e, float y)
+    {
+        return update(e, y);
+    }
+
+    virtual ~PIDT2_Cntrl();
+
+    void    reset(float initValue);
+    void    setCoefficients(float P, float I, float D, float tau_f, float tau_ro, float Ts, float uMin, float uMax);
+    void    setCoeff_P(float P);
+    void    setCoeff_I(float D);
+    void    setCoeff_D(float D);
+    void    scale_PIDT2_param(float scale);
+    float   update(float e);
+    float   update(float e, float y);
+    void    set_limits(float uMin, float uMax);
+    float   get_ulimit();
+    float   get_P_gain();
+		float   get_I();
+		float   get_D();
+    float   get_bd();
+    float   get_ad();
+		float   get_bi();
+		float   get_bf();
+		float   get_af();
+		
+
+private:
+		
+    float   IPart, Dpart, d_old, u_old, uf;
+    float  	P, I, D, tau_f, tau_ro, Ts, uMin, uMax;
+    float  	bi, bd, ad, bf, af;
+    float  	P_init, I_init, D_init;
+
+    void    setCoefficients(float P, float I, float D, float tau_f, float tau_ro, float Ts);
+    float   saturate(float u, float uMin, float uMax);
+
+};
+
+#endif