![](/media/cache/profiles/altb_JGUSAOG.jpg.50x50_q85.jpg)
Mirror actuator for RT2 lab
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
--- 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);
--- 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; -}
--- 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
--- 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; -}
--- 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
--- 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; -}
--- 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); - -};
--- 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;