Diff: PT1_Controller.cpp

Revision:

0:937360eb9f8c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PT1_Controller.cpp	Wed Dec 04 10:24:29 2019 +0000
@@ -0,0 +1,75 @@
+#include "PT1_Controller.h"
+
+PT1_Controller::PT1_Controller(double Kp, double Ts, double Tf)
+{
+    copyControllerPara(Kp, Ts, Tf);
+    calcFilterCoeff();
+    u_max = 1000000.0; // big number
+    u_min = -u_max;
+}
+
+PT1_Controller::PT1_Controller(double Kp, double Ts, double Tf, double u_max)
+{
+    copyControllerPara(Kp, Ts, Tf);
+    calcFilterCoeff();
+    this->u_max = u_max;
+    u_min = -u_max;
+}
+
+PT1_Controller::PT1_Controller(double Kp, double Ts, double Tf, double u_max, double u_min)
+{
+    copyControllerPara(Kp, Ts, Tf);
+    calcFilterCoeff();
+    this->u_max = u_max;
+    this->u_min = u_min;
+}
+
+PT1_Controller::~PT1_Controller() {}
+
+void PT1_Controller::reset()
+{
+    u_kmin1 = 0.0;
+    uf_k = 0.0;
+}
+
+void PT1_Controller::reset(double setVal)
+{
+    u_kmin1 = setVal;
+    uf_k = setVal;
+}
+
+double PT1_Controller::update(double e)
+{
+    // proportional controller
+    double u = Kp * e;
+
+    // constrained low-pass filter
+    uf_k = u*bf + u_kmin1*bf - af*uf_k;
+    u_kmin1 = u;
+    return saturate(uf_k);
+}
+
+void PT1_Controller::copyControllerPara(double Kp, double Ts, double Tf)
+{
+    this->Kp = Kp;
+    this->Ts = Ts;
+    this->Tf = Tf;
+}
+
+void PT1_Controller::calcFilterCoeff()
+{
+    // tustin transformation
+    bf = Ts / (2.0*Tf + Ts);
+    af = (Ts - 2.0*Tf) / (2.0*Tf + Ts);
+}
+
+double PT1_Controller::saturate(double u)
+{
+    if(u > u_max) {
+        u = u_max;
+    } else if(u < u_min) {
+        u = u_min;
+    }
+    return u;
+}
+