涂 桂旺 / AutomationElements

test

Fork of AutomationElements by TVZ Mechatronics Team

PT2.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 "PT2.h"
tbjazic 0:3dd7aeceee65 2
tbjazic 0:3dd7aeceee65 3 PT2::PT2() {
tbjazic 0:3dd7aeceee65 4 u = y = u_p1 = y_p1 = u_p2 = y_p2 = 0;
tbjazic 0:3dd7aeceee65 5 setParameters(1, 1, 0.5, 0.1);
tbjazic 0:3dd7aeceee65 6 }
tbjazic 0:3dd7aeceee65 7
tbjazic 0:3dd7aeceee65 8 PT2::PT2(double K_, double T_1_, double T_2_, double T_d_) {
tbjazic 0:3dd7aeceee65 9 u = y = u_p1 = y_p1 = u_p2 = y_p2 = 0;
tbjazic 0:3dd7aeceee65 10 setParameters(K_, T_1_, T_2_, T_d_);
tbjazic 0:3dd7aeceee65 11 }
tbjazic 0:3dd7aeceee65 12
tbjazic 0:3dd7aeceee65 13 void PT2::setParameters(double K_, double T_1_, double T_2_, 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 T_1 = T_1_;
tbjazic 0:3dd7aeceee65 20 T_2 = T_2_;
tbjazic 0:3dd7aeceee65 21 a = 1 / T_1;
tbjazic 0:3dd7aeceee65 22 b = 1 / T_2;
tbjazic 0:3dd7aeceee65 23 c = exp(-a * T_d);
tbjazic 0:3dd7aeceee65 24 d = exp(-b * T_d);
tbjazic 0:3dd7aeceee65 25 b_1 = -K * ( (a - b)*(c + d) + b*(1 + d) - a*(1 + c) ) / (a - b);
tbjazic 0:3dd7aeceee65 26 b_2 = K * ( (a - b)*c*d + b*d - a*c ) / (a - b);
tbjazic 0:3dd7aeceee65 27 a_1 = -(c + d);
tbjazic 0:3dd7aeceee65 28 a_2 = c*d;
tbjazic 0:3dd7aeceee65 29 }
tbjazic 0:3dd7aeceee65 30
tbjazic 0:3dd7aeceee65 31 double PT2::out() {
tbjazic 0:3dd7aeceee65 32 y = -a_1 * y_p1 - a_2 * y_p2 + b_1 * u_p1 + b_2 * u_p2;
tbjazic 0:3dd7aeceee65 33 y_p2 = y_p1;
tbjazic 0:3dd7aeceee65 34 y_p1 = y;
tbjazic 0:3dd7aeceee65 35 u_p2 = u_p1;
tbjazic 0:3dd7aeceee65 36 u_p1 = u;
tbjazic 0:3dd7aeceee65 37 return y;
tbjazic 0:3dd7aeceee65 38 }
tbjazic 0:3dd7aeceee65 39
tbjazic 0:3dd7aeceee65 40 void PT2::in(double u_) {
tbjazic 0:3dd7aeceee65 41 u = u_;
tbjazic 0:3dd7aeceee65 42 }