Revision 0:3dd7aeceee65, committed 2015-01-22

Comitter:

tbjazic

Date:

Thu Jan 22 11:41:02 2015 +0000

Child:

1:b9e11da0f2eb

Commit message:

Initial commit.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DT1.cpp	Thu Jan 22 11:41:02 2015 +0000
@@ -0,0 +1,34 @@
+#include "DT1.h"
+
+DT1::DT1() {
+    u = y = u_p1 = y_p1 = 0;
+    setParameters(1, 0.5, 0.1);
+}
+
+DT1::DT1(double T_1_, double T_D_, double T_d_) {
+    u = y = u_p1 = y_p1 = 0;
+    setParameters(T_1_, T_D_, T_d_);
+}
+
+void DT1::setParameters(double T_1_, double T_D_, double T_d_) {
+    T_1 = T_1_;
+    T_D = T_D_;
+    if (T_d_ > 0) // only positive sample time values allowed
+        T_d = T_d_;
+    else
+        T_d = 0.1;
+    a_1 = -exp(-T_d/T_1);
+    b_0 = T_D / T_1;
+    b_1 = -b_0;
+}
+
+void DT1::in(double u_) {
+    u = u_;
+}
+
+double DT1::out() {
+    y = -a_1*y_p1 + b_0*u + b_1*u_p1;
+    y_p1 = y;
+    u_p1 = u;
+    return y;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DT1.h	Thu Jan 22 11:41:02 2015 +0000
@@ -0,0 +1,32 @@
+#ifndef DT1_H
+#define DT1_H
+
+#include "mbed.h"
+
+/** Transfer function of a DT1 form.
+ * G(s) = Y(s) / U(s) = T_D s / (1 + T_1 s)
+ *
+ * Author(s): TVZ Mechatronics Team
+ *
+ */
+class DT1 {
+    public:
+        /** Default constructor. 
+         * T_1 = 1, T_D = 0.5, sampleTime = 0.1
+         */
+        DT1();
+        /** DT1 transfer function parameters and sample time in seconds. */
+        DT1(double T_1, double T_D, double sampleTime);
+        /** Update DT1 transfer function parameters and sample time. */
+        void setParameters(double T_1, double T_D, double sampleTime);
+        /** Calculate the output y. */
+        double out();
+        /** Set the input u. */
+        void in(double u);
+    private:
+        double y, y_p1;
+        double u, u_p1;
+        double T_1, T_D, T_d, a_1, b_0, b_1;
+};
+
+#endif // DT1_H
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/I.cpp	Thu Jan 22 11:41:02 2015 +0000
@@ -0,0 +1,30 @@
+#include "I.h"
+
+I::I() {
+    u = y = u_p1 = y_p1 = 0;
+    setParameters(1, 0.1);
+}
+
+I::I(double K_I_, double T_d_) {
+    u = y = u_p1 = y_p1 = 0;
+    setParameters(K_I_, T_d_);
+}
+
+void I::setParameters(double K_I_, double T_d_) {
+    if (T_d_ > 0)
+        T_d = T_d_;
+    else
+        T_d = 0.1;
+    K_I = K_I_;
+}
+
+double I::out() {
+    y = y_p1 + K_I * T_d * u_p1;
+    y_p1 = y;    
+    u_p1 = u;
+    return y;
+}
+
+void I::in(double u_) {
+    u = u_;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/I.h	Thu Jan 22 11:41:02 2015 +0000
@@ -0,0 +1,30 @@
+#ifndef I_H
+#define I_H
+
+#include "mbed.h"
+
+/** Transfer function of an integrator.
+ * G(s) = Y(s) / U(s) = K_I / s
+ *
+ * Author(s): TVZ Mechatronics Team
+ *
+ */
+class I {
+    public:
+        /** Default constructor.
+         * K_I = 1, sampleTime = 0.1
+         */
+        I();
+        /** Integrator gain and sample time in seconds. */
+        I(double K_I, double sampleTime);
+        /** Update integrator gain and sample time. */
+        void setParameters(double K_I, double sampleTime);
+        /** Calculate the output y. */
+        double out();
+        /** Set the input u. */
+        void in(double u);
+    private:
+        double K_I, T_d, y, y_p1, u, u_p1;
+};
+
+#endif // I_H
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PDT1.cpp	Thu Jan 22 11:41:02 2015 +0000
@@ -0,0 +1,35 @@
+#include "PDT1.h"
+
+PDT1::PDT1() {
+    u = y = u_p1 = y_p1 = 0;
+    setParameters(1, 1, 0.5, 0.1);
+}
+
+PDT1::PDT1(double K_, double T_1_, double T_D_, double T_d_) {
+    u = y = u_p1 = y_p1 = 0;
+    setParameters(K_, T_1_, T_D_, T_d_);
+}
+
+void PDT1::setParameters(double K_, double T_1_, double T_D_, double T_d_) {
+    K = K_;
+    T_1 = T_1_;
+    T_D = T_D_;
+    if (T_d_ > 0) // only positive sample time values allowed
+        T_d = T_d_;
+    else
+        T_d = 0.1;
+    a_1 = -exp(-T_d/T_1);
+    b_0 = K * T_D / T_1;
+    b_1 = K * (1 + a_1) - b_0;
+}
+
+void PDT1::in(double u_) {
+    u = u_;
+}
+
+double PDT1::out() {
+    y = -a_1*y_p1 + b_0*u + b_1*u_p1;
+    y_p1 = y;
+    u_p1 = u;
+    return y;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PDT1.h	Thu Jan 22 11:41:02 2015 +0000
@@ -0,0 +1,32 @@
+#ifndef PDT1_H
+#define PDT1_H
+
+#include "mbed.h"
+
+/** Transfer function of a PDT1 form.
+ * G(s) = Y(s) / U(s) = K * (1 + T_D s) / (1 + T_1 s)
+ *
+ * Author(s): TVZ Mechatronics Team
+ *
+ */
+class PDT1 {
+    public:
+        /** Default constructor. 
+         * K = 1, T_1 = 1, T_D = 0.5, sampleTime = 0.1
+         */
+        PDT1();
+        /** PDT1 transfer function parameters and sample time in seconds. */
+        PDT1(double K, double T_1, double T_D, double sampleTime);
+        /** Update PDT1 transfer function parameters and sample time. */
+        void setParameters(double K, double T_1, double T_D, double sampleTime);
+        /** Calculate the output y. */
+        double out();
+        /** Set the input u. */
+        void in(double u);
+    private:
+        double y, y_p1;
+        double u, u_p1;
+        double K, T_1, T_D, T_d, a_1, b_0, b_1;
+};
+
+#endif // PDT1_H
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PI.cpp	Thu Jan 22 11:41:02 2015 +0000
@@ -0,0 +1,72 @@
+#include "PI.h"
+
+PI::PI() {
+    u = y = u_p1 = y_p1 = 0;
+    setParameters(1, 1, 0.1);
+    setOutputLimits(0, 1);
+    setOperationMode(automatic);
+}
+
+PI::PI(double K_R_, double T_I_, double T_d_) {
+    u = y = u_p1 = y_p1 = 0;
+    setParameters(K_R_, T_I_, T_d_);
+    setOutputLimits(0, 1);
+    setOperationMode(automatic);
+}
+
+void PI::setParameters(double K_R_, double T_I_, double T_d_) {
+    if (T_d_ > 0)
+        T_d = T_d_;
+    else
+        T_d = 0.1;
+    if (T_I_ > 0)
+        T_I = T_I_;
+    else
+        T_I = 1;
+    K_R = K_R_;
+}
+
+void PI::setOutputLimits(double lowerOutputLimit, double upperOutputLimit) {
+    if (upperOutputLimit > lowerOutputLimit) {
+        y_min = lowerOutputLimit;
+        y_max = upperOutputLimit;
+    }
+}
+
+void PI::setOperationMode(Mode newOperationMode) {
+    // Bumpless transfer from manual to automatic mode
+    if (operationMode == manual && newOperationMode == automatic) {
+        y = y_p1 = y_manual;
+        u = u_p1 = 0;
+    }
+    // Bumpless transfer from automatic to manual mode
+    else if (operationMode == automatic && newOperationMode == manual) {
+        y_manual = y;
+    }
+    operationMode = newOperationMode;
+}
+
+void PI::setOutputManually(double u_C) {
+    y_manual = u_C;
+}
+
+double PI::out() {
+    if (operationMode == automatic) {
+        y = y_p1 + K_R * u + K_R * (T_d / T_I - 1) * u_p1;
+    } else if (operationMode == manual) {
+        y = y_manual;
+    }
+    
+    if (y > y_max)
+        y = y_max;
+    else if (y < y_min)
+        y = y_min;
+    
+    y_p1 = y;    
+    u_p1 = u;
+    return y;
+}
+
+void PI::in(double u_) {
+    u = u_;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PI.h	Thu Jan 22 11:41:02 2015 +0000
@@ -0,0 +1,42 @@
+#ifndef PI_H
+#define PI_H
+
+#include "mbed.h"
+
+/** Data type for choosing the operation mode of the controller ("automatic" or "manual"). */
+enum Mode {automatic, manual};
+
+/** Transfer function of a PI controller with output limitation, anti-windup and bumpless automatic and manual mode.
+ * G_C(s) = U_C(s) / E(s) = K_C ( 1 + 1 / (T_I s) ) = K_C (1 + T_I s) / (T_I s)
+ *
+ * K_C is the controller gain, and T_I is the controller integral time constant in seconds.
+ *
+ * Author(s): TVZ Mechatronics Team
+ *
+ */
+class PI {
+    public:
+        /** Default constructor.
+         * K_C = 1, T_I = 1 s, sampleTime = 0.1 s.
+         */
+        PI();
+        /** PI controller gain, integral time constant in seconds and sample time in seconds. */
+        PI(double K_C, double T_I, double sampleTime);
+        /** Update PI controller gain, integral time constant and sample time. */
+        void setParameters(double K_C, double T_I, double sampleTime);
+        /** Set the controller lower and upper output limit. */
+        void setOutputLimits(double lowerOutputLimit, double upperOutputLimit);
+        /** Set the operation mode to "automatic" or "manual". */
+        void setOperationMode (Mode operationMode);
+        /** Set the controller output manually. */
+        void setOutputManually(double u_C);
+        /** Calculate the controller output u_C. */
+        double out();
+        /** Set the PI controller input e = SP - PV (setpoint - process value). */
+        void in(double e);
+    private:
+        double K_R, T_I, T_d, y, y_p1, u, u_p1, y_max, y_min, y_manual;
+        Mode operationMode;
+};
+
+#endif // PI_H
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PT1.cpp	Thu Jan 22 11:41:02 2015 +0000
@@ -0,0 +1,33 @@
+#include "PT1.h"
+
+PT1::PT1() {
+    u = y = u_p1 = y_p1 = 0;
+    setParameters(1, 1, 0.1);
+}
+
+PT1::PT1(double K_, double T_1_, double T_d_) {
+    u = y = u_p1 = y_p1 = 0;
+    setParameters(K_, T_1_, T_d_);
+}
+
+void PT1::setParameters(double K_, double T_1_, double T_d_) {
+    if (T_d_ > 0)
+        T_d = T_d_;
+    else
+        T_d = 0.1;
+    K = K_;
+    T_1 = T_1_;
+    a_1 = -exp(-T_d/T_1);
+    b_1 = K * (1 + a_1);    
+}
+
+double PT1::out() {
+    y = -a_1 * y_p1 + b_1 * u_p1;
+    y_p1 = y;    
+    u_p1 = u;
+    return y;
+}
+
+void PT1::in(double u_) {
+    u = u_;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PT1.h	Thu Jan 22 11:41:02 2015 +0000
@@ -0,0 +1,30 @@
+#ifndef PT1_H
+#define PT1_H
+
+#include "mbed.h"
+
+/** Transfer function of a PT1 form.
+ * G(s) = Y(s) / U(s) = K / (1 + T_1 s)
+ *
+ * Author(s): TVZ Mechatronics Team
+ *
+ */
+class PT1 {
+    public:
+        /** Default constructor.
+         * K = 1, T_1 = 1, sampleTime = 0.1
+         */
+        PT1();
+        /** PT1 transfer function parameters and sample time in seconds. */
+        PT1(double K, double T_1, double sampleTime);
+        /** Update PT1 transfer function parameters and sample time. */
+        void setParameters(double K, double T_1, double sampleTime);
+        /** Calculate the output y. */
+        double out();
+        /** Set the input u. */
+        void in(double u);
+    private:
+        double K, T_1, T_d, a_1, b_1, y, y_p1, u, u_p1;
+};
+
+#endif // PT1_H
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PT2.cpp	Thu Jan 22 11:41:02 2015 +0000
@@ -0,0 +1,42 @@
+#include "PT2.h"
+
+PT2::PT2() {
+    u = y = u_p1 = y_p1 = u_p2 = y_p2 = 0;
+    setParameters(1, 1, 0.5, 0.1);
+}
+
+PT2::PT2(double K_, double T_1_, double T_2_, double T_d_) {
+    u = y = u_p1 = y_p1 = u_p2 = y_p2 = 0;
+    setParameters(K_, T_1_, T_2_, T_d_);
+}
+
+void PT2::setParameters(double K_, double T_1_, double T_2_, double T_d_) {
+    if (T_d_ > 0)
+        T_d = T_d_;
+    else
+        T_d = 0.1;
+    K = K_;
+    T_1 = T_1_;
+    T_2 = T_2_;
+    a = 1 / T_1;
+    b = 1 / T_2;
+    c = exp(-a * T_d);
+    d = exp(-b * T_d);
+    b_1 = -K * ( (a - b)*(c + d) + b*(1 + d) - a*(1 + c) ) / (a - b);
+    b_2 = K * ( (a - b)*c*d + b*d - a*c ) / (a - b);
+    a_1 = -(c + d);
+    a_2 = c*d;
+}
+
+double PT2::out() {
+    y = -a_1 * y_p1 - a_2 * y_p2 + b_1 * u_p1 + b_2 * u_p2;
+    y_p2 = y_p1;
+    y_p1 = y;   
+    u_p2 = u_p1; 
+    u_p1 = u;
+    return y;
+}
+
+void PT2::in(double u_) {
+    u = u_;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PT2.h	Thu Jan 22 11:41:02 2015 +0000
@@ -0,0 +1,30 @@
+#ifndef PT2_H
+#define PT2_H
+
+#include "mbed.h"
+
+/** Transfer function of a PT2 form.
+ * G(s) = Y(s) / U(s) = K / ( (1 + T_1 s) * (1 + T_2 s) )
+ *
+ * Author(s): TVZ Mechatronics Team
+ *
+ */
+class PT2 {
+    public:
+        /** Default constructor.
+         * K = 1, T_1 = 1 s, T_2 = 0.5 s, sampleTime = 0.1 s.
+         */
+        PT2();
+        /** PT2 transfer function parameters and sample time in seconds. */
+        PT2(double K, double T_1, double T_2, double sampleTime);
+        /** Update PT2 transfer function parameters and sample time. */
+        void setParameters(double K, double T_1, double T_2, double sampleTime);
+        /** Calculate the output y. */
+        double out();
+        /** Set the input u. */
+        void in(double u);
+    private:
+        double K, T_1, T_2, T_d, a, b, c, d, a_1, a_2, b_1, b_2, y, y_p1, y_p2, u, u_p1, u_p2;
+};
+
+#endif // PT2_H
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PT2cc.cpp	Thu Jan 22 11:41:02 2015 +0000
@@ -0,0 +1,43 @@
+#include "PT2cc.h"
+
+PT2cc::PT2cc() {
+    u = y = u_p1 = y_p1 = u_p2 = y_p2 = 0;
+    setParameters(1, 0.707, 1, 0.1);
+}
+
+PT2cc::PT2cc(double K_, double zeta_, double w_n_, double T_d_) {
+    u = y = u_p1 = y_p1 = u_p2 = y_p2 = 0;
+    setParameters(K_, zeta_, w_n_, T_d_);
+}
+
+void PT2cc::setParameters(double K_, double zeta_, double w_n_, double T_d_) {
+    if (T_d_ > 0)
+        T_d = T_d_;
+    else
+        T_d = 0.1;
+    K = K_;
+    zeta = zeta_;
+    w_n = w_n_;
+    a = acos(zeta);
+    b = sqrt(1 - zeta*zeta);
+    c = exp(-zeta * w_n * T_d);
+    d = sin(w_n * b * T_d - a);
+    e = cos(w_n * b * T_d);
+    b_1 = K * (1 - 2*c*e - c*d/b);
+    b_2 = K * (c*c + c*d/b);
+    a_1 = -2*c*e;
+    a_2 = c*c;
+}
+
+double PT2cc::out() {
+    y = -a_1 * y_p1 - a_2 * y_p2 + b_1 * u_p1 + b_2 * u_p2;
+    y_p2 = y_p1;
+    y_p1 = y;   
+    u_p2 = u_p1; 
+    u_p1 = u;
+    return y;
+}
+
+void PT2cc::in(double u_) {
+    u = u_;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PT2cc.h	Thu Jan 22 11:41:02 2015 +0000
@@ -0,0 +1,30 @@
+#ifndef PT2cc_H
+#define PT2cc_H
+
+#include "mbed.h"
+
+/** Transfer function of a general PT2 form. cc stands for conjugate-complex.
+ * G(s) = Y(s) / U(s) = K w_n^2 / ( s^2 + 2 zeta w_n + w_n^2 )
+ *
+ * Author(s): TVZ Mechatronics Team
+ *
+ */
+class PT2cc {
+    public:
+        /** Default constructor.
+         * K = 1, zeta = 0.707, w_n = 1 rad/s, sampleTime = 0.1 s.
+         */
+        PT2cc();
+        /** PT2cc transfer function parameters K, zeta, w_n and sample time in seconds. */
+        PT2cc(double K, double zeta, double w_n, double sampleTime);
+        /** Update PT2cc transfer function parameters and sample time. */
+        void setParameters(double K, double zeta, double w_n, double sampleTime);
+        /** Calculate the output y. */
+        double out();
+        /** Set the input u. */
+        void in(double u);
+    private:
+        double K, zeta, w_n, T_d, a, b, c, d, e, a_1, a_2, b_1, b_2, y, y_p1, y_p2, u, u_p1, u_p2;
+};
+
+#endif // PT2cc_H
\ No newline at end of file