Ruprecht Altenburger / Mbed OS mirror_actuator_stud

template for students for mirror actuator

Dependencies:   FastPWM

Lib_Cntrl/PID_Cntrl.cpp@3:d672a96eeecc, 2022-03-06 (annotated)

Committer:
altb2
Date:
Sun Mar 06 08:57:28 2022 +0000
Revision:
3:d672a96eeecc
Parent:
2:c4c4cc1bff45 
MirrorActuator Stud FS21

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 0:d2e117716219 1 /*
altb2 0:d2e117716219 2 */
altb2 0:d2e117716219 3
altb2 0:d2e117716219 4 #include "PID_Cntrl.h"
altb2 0:d2e117716219 5
altb2 1:a7fc1afe0575 6 // Matlab
altb2 1:a7fc1afe0575 7 // Tn = .005;
altb2 1:a7fc1afe0575 8 // Gpi= tf([Tn 1],[Tn 0]);
altb2 1:a7fc1afe0575 9 // Kp = 0.0158;
altb2 1:a7fc1afe0575 10 // pid(Kp*Gpi);
altb2 1:a7fc1afe0575 11
altb2 0:d2e117716219 12 PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax)
altb2 0:d2e117716219 13 {
altb2 0:d2e117716219 14 // ------------------
altb2 1:a7fc1afe0575 15 this->P = P;
altb2 1:a7fc1afe0575 16 this->I = I;
altb2 2:c4c4cc1bff45 17 this->D = D;
altb2 2:c4c4cc1bff45 18 this->tau_f = tau_f;
altb2 1:a7fc1afe0575 19 this->Ts = Ts;
altb2 1:a7fc1afe0575 20 this->uMin = uMin;
altb2 1:a7fc1afe0575 21 this->uMax = uMax;
altb2 1:a7fc1afe0575 22 reset(0);
altb2 0:d2e117716219 23 }
altb2 0:d2e117716219 24
altb2 0:d2e117716219 25 PID_Cntrl::~PID_Cntrl() {}
altb2 0:d2e117716219 26
altb2 0:d2e117716219 27 void PID_Cntrl::reset(float initValue)
altb2 0:d2e117716219 28 {
altb2 0:d2e117716219 29 // -----------------------
altb2 1:a7fc1afe0575 30 Ipart = initValue;
altb2 2:c4c4cc1bff45 31 Dpart = 0.0f;
altb2 2:c4c4cc1bff45 32 e_old = 0.0f;
altb2 0:d2e117716219 33 }
altb2 0:d2e117716219 34
altb2 0:d2e117716219 35
altb2 0:d2e117716219 36 float PID_Cntrl::update(float e)
altb2 0:d2e117716219 37 {
altb2 0:d2e117716219 38 // the main update
altb2 1:a7fc1afe0575 39
altb2 1:a7fc1afe0575 40 Ipart += I * Ts * e;
altb2 2:c4c4cc1bff45 41 Dpart = tau_f / (Ts+tau_f) * Dpart + D/(Ts+tau_f)*(e-e_old);
altb2 2:c4c4cc1bff45 42 e_old = e;
altb2 1:a7fc1afe0575 43 Ipart = saturate(Ipart);
altb2 2:c4c4cc1bff45 44 return saturate(P * e + Ipart + Dpart);
altb2 1:a7fc1afe0575 45 }
altb2 1:a7fc1afe0575 46
altb2 1:a7fc1afe0575 47 float PID_Cntrl::saturate(float x)
altb2 1:a7fc1afe0575 48 {
altb2 1:a7fc1afe0575 49 if(x > uMax)
altb2 1:a7fc1afe0575 50 return uMax;
altb2 1:a7fc1afe0575 51 else if(x < uMin)
altb2 1:a7fc1afe0575 52 return uMin;
altb2 1:a7fc1afe0575 53 return x;
altb2 0:d2e117716219 54 }