Diff: DT1_Cntrl.cpp

Revision:

7:cb1492f4f2c6

Child:

8:3a8d1218022c

diff -r 694fe8894215 -r cb1492f4f2c6 DT1_Cntrl.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DT1_Cntrl.cpp	Wed Sep 18 09:56:35 2019 +0000
@@ -0,0 +1,55 @@
+/*  
+    DT1 Controller class 
+    everything is calculated in float
+    
+                     z - 1             
+      G(s) = Kd/tau -------          D corresponds Kd in Matlab-formlism pid(...)
+                     z - p              
+*/
+
+#include "DT1_Cntrl.h"
+using namespace std;
+
+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;
+    reset(0.0f);
+}
+
+DT1_Cntrl::~DT1_Cntrl() {}
+
+void DT1_Cntrl::reset(float initValue)
+{
+    eold = initValue;
+    yold = 0.0;
+}
+
+void DT1_Cntrl::setCoefficients(float D, float tau_f, float Ts)
+{
+    this->p = 1.0 - 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->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;
+    return Dpart;
+}
+
+void DT1_Cntrl::set_limits(float ll, float ul)
+{
+    this->uMin = ll;
+    this->uMax = ul;
+}
+