涂 桂旺 / AutomationElements

test

Fork of AutomationElements by TVZ Mechatronics Team

PT2cc.cpp@2:a45cbb512c99, 2017-11-25 (annotated)

Committer:
tgw
Date:
Sat Nov 25 02:03:00 2017 +0000
Revision:
2:a45cbb512c99
Parent:
0:3dd7aeceee65 
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tbjazic 0:3dd7aeceee65 1 #include "PT2cc.h"
tbjazic 0:3dd7aeceee65 2
tbjazic 0:3dd7aeceee65 3 PT2cc::PT2cc() {
tbjazic 0:3dd7aeceee65 4 u = y = u_p1 = y_p1 = u_p2 = y_p2 = 0;
tbjazic 0:3dd7aeceee65 5 setParameters(1, 0.707, 1, 0.1);
tbjazic 0:3dd7aeceee65 6 }
tbjazic 0:3dd7aeceee65 7
tbjazic 0:3dd7aeceee65 8 PT2cc::PT2cc(double K_, double zeta_, double w_n_, double T_d_) {
tbjazic 0:3dd7aeceee65 9 u = y = u_p1 = y_p1 = u_p2 = y_p2 = 0;
tbjazic 0:3dd7aeceee65 10 setParameters(K_, zeta_, w_n_, T_d_);
tbjazic 0:3dd7aeceee65 11 }
tbjazic 0:3dd7aeceee65 12
tbjazic 0:3dd7aeceee65 13 void PT2cc::setParameters(double K_, double zeta_, double w_n_, double T_d_) {
tbjazic 0:3dd7aeceee65 14 if (T_d_ > 0)
tbjazic 0:3dd7aeceee65 15 T_d = T_d_;
tbjazic 0:3dd7aeceee65 16 else
tbjazic 0:3dd7aeceee65 17 T_d = 0.1;
tbjazic 0:3dd7aeceee65 18 K = K_;
tbjazic 0:3dd7aeceee65 19 zeta = zeta_;
tbjazic 0:3dd7aeceee65 20 w_n = w_n_;
tbjazic 0:3dd7aeceee65 21 a = acos(zeta);
tbjazic 0:3dd7aeceee65 22 b = sqrt(1 - zeta*zeta);
tbjazic 0:3dd7aeceee65 23 c = exp(-zeta * w_n * T_d);
tbjazic 0:3dd7aeceee65 24 d = sin(w_n * b * T_d - a);
tbjazic 0:3dd7aeceee65 25 e = cos(w_n * b * T_d);
tbjazic 0:3dd7aeceee65 26 b_1 = K * (1 - 2*c*e - c*d/b);
tbjazic 0:3dd7aeceee65 27 b_2 = K * (c*c + c*d/b);
tbjazic 0:3dd7aeceee65 28 a_1 = -2*c*e;
tbjazic 0:3dd7aeceee65 29 a_2 = c*c;
tbjazic 0:3dd7aeceee65 30 }
tbjazic 0:3dd7aeceee65 31
tbjazic 0:3dd7aeceee65 32 double PT2cc::out() {
tbjazic 0:3dd7aeceee65 33 y = -a_1 * y_p1 - a_2 * y_p2 + b_1 * u_p1 + b_2 * u_p2;
tbjazic 0:3dd7aeceee65 34 y_p2 = y_p1;
tbjazic 0:3dd7aeceee65 35 y_p1 = y;
tbjazic 0:3dd7aeceee65 36 u_p2 = u_p1;
tbjazic 0:3dd7aeceee65 37 u_p1 = u;
tbjazic 0:3dd7aeceee65 38 return y;
tbjazic 0:3dd7aeceee65 39 }
tbjazic 0:3dd7aeceee65 40
tbjazic 0:3dd7aeceee65 41 void PT2cc::in(double u_) {
tbjazic 0:3dd7aeceee65 42 u = u_;
tbjazic 0:3dd7aeceee65 43 }