PT2cc.cpp

00001 #include "PT2cc.h"
00002 
00003 PT2cc::PT2cc() {
00004     u = y = u_p1 = y_p1 = u_p2 = y_p2 = 0;
00005     setParameters(1, 0.707, 1, 0.1);
00006 }
00007 
00008 PT2cc::PT2cc(double K_, double zeta_, double w_n_, double T_d_) {
00009     u = y = u_p1 = y_p1 = u_p2 = y_p2 = 0;
00010     setParameters(K_, zeta_, w_n_, T_d_);
00011 }
00012 
00013 void PT2cc::setParameters(double K_, double zeta_, double w_n_, double T_d_) {
00014     if (T_d_ > 0)
00015         T_d = T_d_;
00016     else
00017         T_d = 0.1;
00018     K = K_;
00019     zeta = zeta_;
00020     w_n = w_n_;
00021     a = acos(zeta);
00022     b = sqrt(1 - zeta*zeta);
00023     c = exp(-zeta * w_n * T_d);
00024     d = sin(w_n * b * T_d - a);
00025     e = cos(w_n * b * T_d);
00026     b_1 = K * (1 - 2*c*e - c*d/b);
00027     b_2 = K * (c*c + c*d/b);
00028     a_1 = -2*c*e;
00029     a_2 = c*c;
00030 }
00031 
00032 double PT2cc::out() {
00033     y = -a_1 * y_p1 - a_2 * y_p2 + b_1 * u_p1 + b_2 * u_p2;
00034     y_p2 = y_p1;
00035     y_p1 = y;   
00036     u_p2 = u_p1; 
00037     u_p1 = u;
00038     return y;
00039 }
00040 
00041 void PT2cc::in(double u_) {
00042     u = u_;
00043 }