Diff: Lib_Cntrl/PID_Cntrl.cpp

Revision:

1:a7fc1afe0575

Parent:

0:d2e117716219

Child:

2:c4c4cc1bff45

--- a/Lib_Cntrl/PID_Cntrl.cpp	Sun May 02 19:32:30 2021 +0000
+++ b/Lib_Cntrl/PID_Cntrl.cpp	Mon May 17 12:03:40 2021 +0000
@@ -3,22 +3,46 @@
 
 #include "PID_Cntrl.h"
 
+// Matlab
+// Tn = .005;
+// Gpi= tf([Tn 1],[Tn 0]);
+// Kp = 0.0158;
+// pid(Kp*Gpi);
+
 PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax)
 {
     // ------------------
+    this->P = P;
+    this->I = I;
+    this->Ts = Ts;
+    this->uMin = uMin;
+    this->uMax = uMax;
+    reset(0);
 }
 
 PID_Cntrl::~PID_Cntrl() {}
 
 void PID_Cntrl::reset(float initValue)
 {
-
     // -----------------------
+    Ipart = initValue;
 }
 
 
 float PID_Cntrl::update(float e)
 {
     // the main update 
-    return 0.0;
+    
+    Ipart += I * Ts * e;
+    Ipart = saturate(Ipart);
+    return saturate(P * e + Ipart);
+}
+
+float PID_Cntrl::saturate(float x)
+{
+if(x > uMax)
+    return uMax;
+else if(x < uMin)
+    return uMin;
+return x;
 }
\ No newline at end of file