Revision 16:28b6bb8a4b7f, committed 2021-05-02

Comitter:

altb2

Date:

Sun May 02 08:55:44 2021 +0000

Parent:

15:9f32f64eee5b

Commit message:

Final commit 4 students

Changed in this revision

diff -r 9f32f64eee5b -r 28b6bb8a4b7f ControllerLoop.h
--- a/ControllerLoop.h	Sun May 02 08:17:06 2021 +0000
+++ b/ControllerLoop.h	Sun May 02 08:55:44 2021 +0000
@@ -5,7 +5,6 @@
 #include "LinearCharacteristics.h"
 #include "ThreadFlag.h"
 #include "path_1d.h"
-#include "PIDT2_Cntrl.h"
 #include "Unwrapper_2pi.h"
 #include "Mirror_Kinematic.h"
 #include "data_structs.h"
@@ -51,7 +50,6 @@
     void sendSignal();
     bool is_initialized;
     void find_index(void);
-    PIDT2_Cntrl v_cntrl[2];
     Unwrapper_2pi uw2pi1;
     Unwrapper_2pi uw2pi2;
     float pos_cntrl(float);

diff -r 9f32f64eee5b -r 28b6bb8a4b7f Library_Cntrl/PIDT2_Cntrl.cpp
--- 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;
-}

diff -r 9f32f64eee5b -r 28b6bb8a4b7f Library_Cntrl/PIDT2_Cntrl.h
--- a/Library_Cntrl/PIDT2_Cntrl.h	Sun May 02 08:17:06 2021 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,57 +0,0 @@
-#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

diff -r 9f32f64eee5b -r 28b6bb8a4b7f Library_Cntrl/PID_Cntrl.cpp
--- a/Library_Cntrl/PID_Cntrl.cpp	Sun May 02 08:17:06 2021 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,69 +0,0 @@
-/*  
-    PI Controller class with anti windup reset in biquad transposed direct form 2
-    see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html
-    everything is calculated in double
-    
-                     Ts         z - 1             
-      G(s) = P + I ------- + D -------          D corresponds Kd/Tf in Matlab-formlism pid(...)
-                    z - 1       z - p              
-*/
-
-#include "PID_Cntrl.h"
-using namespace std;
-
-PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax)
-{
-    setCoefficients(P, I, D, tau_f, Ts);
-    this->uMin = (double)uMin;
-    this->uMax = (double)uMax;
-    reset(0.0f);
-}
-
-PID_Cntrl::~PID_Cntrl() {}
-
-void PID_Cntrl::reset(float initValue)
-{
-    Iold = (double)initValue;
-    eold = 0.0;yold = 0.0;
-    del = 0.0;
-}
-
-void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float Ts)
-{
-    this->p = 1.0 - (double)Ts/(double)tau_f;
-    this->P = P;
-    this->I = I;
-    this->D = D;
-    this->Ts = Ts;
-    if(P!=0)
-        this->Ka=1/P;
-    else
-        this->Ka=1.0f;
-    
-}
-
-float PID_Cntrl::doStep(double e)
-{
-    double Ipart = Iold+I*Ts*(e-del);
-    double Dpart = D*(e-eold)+p*yold;
-    double u = P*e + Dpart  + Ipart;          // unconstrained output
-    double uc = u;                // constrained output
-    if(u > uMax) uc = uMax;
-    else if(u < uMin) uc = uMin;
-    del=(u-uc)*Ka;
-    eold=e;
-    Iold=Ipart;
-    yold=Dpart;
-    return (float)uc;
-}
-
-void PID_Cntrl::set_limits(double ll, double ul)
-{
-    this->uMin = (double)ll;
-    this->uMax = (double)ul;
-}
-
-float PID_Cntrl::get_ulimit(void)
-{
-    return (float)this->uMax;
-}

diff -r 9f32f64eee5b -r 28b6bb8a4b7f Library_Cntrl/PID_Cntrl.h
--- a/Library_Cntrl/PID_Cntrl.h	Sun May 02 08:17:06 2021 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,34 +0,0 @@
-#ifndef PID_CNTRL_H_
-#define PID_CNTRL_H_
-
-class PID_Cntrl
-{
-public:
-
-    PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax);
-
-    float operator()(float error) {
-        return doStep((double)error);
-    }
-
-    virtual     ~PID_Cntrl();
-
-    void        reset(float initValue);
-    float       doStep(double error);
-    void        set_limits(double ,double );
-    float       get_ulimit(void);
-    
-private:
-
-    double Iold;
-    double eold,yold,del;
-    double uMax;
-    double uMin;
-    double Ts;
-    double P,I,D;
-    double p, Ka;
-    void setCoefficients(float P, float I, float D, float tau_f, float Ts);
-
-};
-
-#endif
\ No newline at end of file

diff -r 9f32f64eee5b -r 28b6bb8a4b7f Library_Cntrl/PI_Cntrl.cpp
--- a/Library_Cntrl/PI_Cntrl.cpp	Sun May 02 08:17:06 2021 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,61 +0,0 @@
-/*  
-    PI Controller class with anti windup reset in biquad transposed direct form 2
-    see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html
-    everything is calculated in double
-    
-                  Tn*s + 1                      
-      G(s) = Kp -------------  with s ~ (1 - z^-1)/Ts
-                    Ts*s                     
-*/
-
-#include "PI_Cntrl.h"
-
-using namespace std;
-
-PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts)
-{
-    setCoefficients(Kp, Tn, Ts);
-    uMax = 10000000000.0;
-    uMin = -uMax;
-    reset(0.0f);
-}
-
-PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax)
-{
-    setCoefficients(Kp, Tn, Ts);
-    this->uMax = (double)uMax;
-    uMin = -(double)uMax;
-    reset(0.0f);
-}
-
-PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax, float uMin)
-{
-    setCoefficients(Kp, Tn, Ts);
-    this->uMax = (double)uMax;
-    this->uMin = (double)uMin;
-    reset(0.0f);
-}
-
-PI_Cntrl::~PI_Cntrl() {}
-
-void PI_Cntrl::reset(float initValue)
-{
-    s = (double)initValue;
-}
-
-void PI_Cntrl::setCoefficients(float Kp, float Tn, float Ts)
-{
-    b0 = (double)Kp*(1.0 + (double)Ts/(double)Tn);
-    b1 = -(double)Kp;
-    b2 = (double)Ts/(double)Tn;    
-}
-
-float PI_Cntrl::doStep(double e)
-{
-    double u = b0*e + s;          // unconstrained output
-    double uc = u;                // constrained output
-    if(u > uMax) uc = uMax;
-    else if(u < uMin) uc = uMin;
-    s = b1*e + u - b2*(u - uc);
-    return (float)uc;
-}

diff -r 9f32f64eee5b -r 28b6bb8a4b7f Library_Cntrl/PI_Cntrl.h
--- a/Library_Cntrl/PI_Cntrl.h	Sun May 02 08:17:06 2021 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,29 +0,0 @@
-class PI_Cntrl
-{
-public:
-
-    PI_Cntrl(float Kp, float Tn, float Ts);
-    PI_Cntrl(float Kp, float Tn, float Ts, float uMax);
-    PI_Cntrl(float Kp, float Tn, float Ts, float uMax, float uMin);
-
-    float operator()(float error) {
-        return doStep((double)error);
-    }
-
-    virtual     ~PI_Cntrl();
-
-    void        reset(float initValue);
-    float       doStep(double error);
-
-private:
-
-    double b0;
-    double b1;
-    double b2;
-    double s;
-    double uMax;
-    double uMin;
-    
-    void        setCoefficients(float Kp, float Tn, float Ts);
-
-};

diff -r 9f32f64eee5b -r 28b6bb8a4b7f main.cpp
--- a/main.cpp	Sun May 02 08:17:06 2021 +0000
+++ b/main.cpp	Sun May 02 08:55:44 2021 +0000
@@ -9,7 +9,6 @@
 #include "DiffCounter.h"
 #include "IIR_filter.h"
 #include "LinearCharacteristics.h"
-#include "PID_Cntrl.h"
 #include "Unwrapper_2pi.h"
 #include "path_1d.h"
 #include "GPA.h"
@@ -63,7 +62,7 @@
 path_1d *current_path;
 // --------- GPA -----------------------------
 //init values: f0,   f1, nbPts, A0, A1, Ts
-GPA      myGPA(5 , 2450,    40, 35, 25, Ts);
+GPA      myGPA(5 , 2450,    40, 45, 35, Ts);
 //------------------------------------------------------------------------------
 // --------- Mirror kinematik, define values, trafos etc there
 Mirror_Kinematic mk;