File content as of revision 2:a45cbb512c99:

#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_;
}