Diff: PT2cc.cpp

Revision:

0:3dd7aeceee65

--- /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