涂 桂旺 / Mbed OS car_scr

test

Dependencies:   ESP8266 HCSR04 PID

Fork of car_test_v1 by 涂 桂旺

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