涂 桂旺 / Mbed OS car_scr

test

Dependencies:   ESP8266 HCSR04 PID

Fork of car_test_v1 by 涂 桂旺

AutomationElement/PT2cc.cpp@3:9e51de1050a1, 2017-11-25 (annotated)

Committer:
tgw
Date:
Sat Nov 25 03:36:58 2017 +0000
Revision:
3:9e51de1050a1
test

Who changed what in which revision?

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