Ruprecht Altenburger / Mbed OS mirror_actuator_stud

template for students for mirror actuator

Dependencies:   FastPWM

Diff: Lib_Cntrl/PID_Cntrl.cpp

Revision:
2:c4c4cc1bff45
Parent:
1:a7fc1afe0575
--- a/Lib_Cntrl/PID_Cntrl.cpp	Mon May 17 12:03:40 2021 +0000
+++ b/Lib_Cntrl/PID_Cntrl.cpp	Tue Oct 19 06:46:33 2021 +0000
@@ -14,6 +14,8 @@
     // ------------------
     this->P = P;
     this->I = I;
+    this->D = D;
+    this->tau_f = tau_f;
     this->Ts = Ts;
     this->uMin = uMin;
     this->uMax = uMax;
@@ -26,6 +28,8 @@
 {
     // -----------------------
     Ipart = initValue;
+    Dpart = 0.0f;
+    e_old = 0.0f;
 }
 
 
@@ -34,8 +38,10 @@
     // the main update 
     
     Ipart += I * Ts * e;
+    Dpart = tau_f / (Ts+tau_f) * Dpart + D/(Ts+tau_f)*(e-e_old);
+    e_old = e;
     Ipart = saturate(Ipart);
-    return saturate(P * e + Ipart);
+    return saturate(P * e + Ipart + Dpart);
 }
 
 float PID_Cntrl::saturate(float x)