Diff: DT1_Cntrl.cpp

Revision:

9:074f4f94b584

Parent:

8:3a8d1218022c

--- a/DT1_Cntrl.cpp	Thu Sep 19 08:51:03 2019 +0000
+++ b/DT1_Cntrl.cpp	Fri Sep 20 09:26:23 2019 +0000
@@ -1,10 +1,10 @@
-/*  
-    DT1 Controller class 
+/*
+    DT1 Controller class
     everything is calculated in float
-    
-                     z - 1             
-      G(s) = Kd/tau -------          D corresponds Kd in Matlab-formlism pid(...)
-                     z - p              
+
+                     z - 1
+      G(s) = Kd/tau ------- , p = 1 - Ts/tau   D corresponds Kd in Matlab-formlism pid(...)
+                     z - p
 */
 
 #include "DT1_Cntrl.h"
@@ -13,45 +13,45 @@
 DT1_Cntrl::DT1_Cntrl(float D, float tau_f, float Ts, float uMin, float uMax)
 {
     setCoefficients(D, tau_f, Ts);
-    this->uMin = uMin;
-    this->uMax = uMax;
+    set_limits(uMin, uMax);
     reset(0.0f);
 }
 
 DT1_Cntrl::~DT1_Cntrl() {}
 
+
 void DT1_Cntrl::reset(float initValue)
 {
     eold = initValue;
-    yold = 0.0;
+    yold = 0.0f;
+}
+
+float DT1_Cntrl::doStep(float e)
+{
+    float Dpart = D * ( e - eold ) + p * yold;
+    if(Dpart > uMax) Dpart = uMax;
+    else if(Dpart < uMin) Dpart = uMin;
+    eold = e;
+    yold = Dpart;
+    return Dpart;
+}
+
+void DT1_Cntrl::set_limits(float uMin, float uMax)
+{
+    this->uMin = uMin;
+    this->uMax = uMax;
+}
+
+void DT1_Cntrl::setCoeff_D(float D)
+{
+    this->D = D/tau_f;
 }
 
 void DT1_Cntrl::setCoefficients(float D, float tau_f, float Ts)
 {
     this->p = 1.0f - Ts/tau_f;
     this->tau_f = tau_f;
-    this->D_ = D/tau_f;             // modified D, now D is consistent with Matlab PID
-    this->D__init = D/tau_f;        // modified D, now D is consistent with Matlab PID
+    this->D = D/tau_f;             // modified D, now D is consistent with Matlab PID
+    this->D_init = D/tau_f;        // modified D, now D is consistent with Matlab PID
     this->Ts = Ts;
-}
-void DT1_Cntrl::setCoeff_D(float D)
-{
-    this->D_ = D/this->tau_f;
-}
-
-float DT1_Cntrl::doStep(float e)
-{
-    float Dpart = D_*(e-eold)+p*yold;
-    if(Dpart > uMax) Dpart = uMax;
-    else if(Dpart < uMin) Dpart = uMin;
-    eold=e;
-    yold=Dpart;
-    return Dpart;
-}
-
-void DT1_Cntrl::set_limits(float ll, float ul)
-{
-    this->uMin = ll;
-    this->uMax = ul;
-}
-
+}
\ No newline at end of file