Diff: PID_Cntrl.cpp

Revision:

2:e7c9e50a2e46

Parent:

0:d49418189c5c

Child:

3:27595dd9e433

--- a/PID_Cntrl.cpp	Mon Mar 04 11:26:47 2019 +0000
+++ b/PID_Cntrl.cpp	Fri Jun 14 07:05:55 2019 +0000
@@ -1,11 +1,11 @@
 /*  
     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
+    everything is calculated in float
     
-                     Ts         z - 1             
-      G(s) = P + I ------- + D -------          D corresponds Kd/Tf in Matlab-formlism pid(...)
-                    z - 1       z - p              
+                     Ts             z - 1             
+      G(s) = P + I ------- + D/tau -------          D corresponds Kd in Matlab-formlism pid(...)
+                    z - 1           z - p              
 */
 
 #include "PID_Cntrl.h"
@@ -14,8 +14,8 @@
 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;
+    this->uMin = uMin;
+    this->uMax = uMax;
     reset(0.0f);
 }
 
@@ -23,17 +23,17 @@
 
 void PID_Cntrl::reset(float initValue)
 {
-    Iold = (double)initValue;
+    Iold = 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 = 1.0 - Ts/tau_f;
     this->P = P;
     this->I = I;
-    this->D = D;
+    this->D_ = D/tau_f;     // modified D, now D is consistent with Matlab PID
     this->Ts = Ts;
     if(P!=0)
         this->Ka=1/P;
@@ -42,28 +42,28 @@
     
 }
 
-float PID_Cntrl::doStep(double e)
+float PID_Cntrl::doStep(float 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
+    float Ipart = Iold+I*Ts*(e-del);
+    float Dpart = D_*(e-eold)+p*yold;
+    float u = P*e + Dpart  + Ipart;          // unconstrained output
+    float 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;
+    return uc;
 }
 
-void PID_Cntrl::set_limits(double ll, double ul)
+void PID_Cntrl::set_limits(float ll, float ul)
 {
-    this->uMin = (double)ll;
-    this->uMax = (double)ul;
+    this->uMin = ll;
+    this->uMax = ul;
 }
 
 float PID_Cntrl::get_ulimit(void)
 {
-    return (float)this->uMax;
+    return this->uMax;
 }
\ No newline at end of file