Revision 43dfd684
Added by Pierre-Loïc Garoche about 4 years ago
regression_tests/lustre_files/success/lego_robot/controller/lego_anti_windup.lus | ||
---|---|---|
102 | 102 |
speedC = real_to_int(pwm_l); |
103 | 103 |
tel |
104 | 104 |
|
105 |
|
|
106 |
node lego_anti_windup_real (in1, in2, in3, in4, cntA, cntB, cntC, battery_voltage : real; gyro_offset : real) |
|
107 |
returns (speedA, speedB, speedC : real); |
|
108 |
var gyro, cmd_forward, cmd_turn, theta_m_l, theta_m_r, pwm_turn, theta_ref, x_11_ref, x_12_ref, x_13_ref, x_14_ref, theta, x_11, x_12, x_13, x_14, errtheta, err, pwm_l, pwm_r, anti_windup: real; |
|
109 |
let |
|
110 |
cmd_forward = if in2 <= 25. then -100. else 0.; |
|
111 |
cmd_turn = 0.; |
|
112 |
gyro = in3; |
|
113 |
theta_m_l = cntC; |
|
114 |
theta_m_r = cntB; |
|
115 |
|
|
116 |
(pwm_turn, theta_ref, x_11_ref, x_12_ref, x_13_ref, x_14_ref) = cal_reference(cmd_forward, cmd_turn); |
|
117 |
|
|
118 |
(theta, x_11, x_12, x_13, x_14) = cal_x1(gyro, gyro_offset, theta_m_l, theta_m_r); |
|
119 |
|
|
120 |
errtheta = discrete_integrator(exec_period, (theta_ref-theta)*k_i + 0. -> pre(anti_windup*0.1)); |
|
121 |
|
|
122 |
err = errtheta + k_f_1*(x_11_ref-x_11) + k_f_2*(x_12_ref-x_12)+ k_f_3*(x_13_ref-x_13) +k_f_4*(x_14_ref-x_14); |
|
123 |
|
|
124 |
(pwm_l,pwm_r, anti_windup) = cal_pwm(theta_m_l, theta_m_r, pwm_turn, err, battery_voltage); |
|
125 |
|
|
126 |
speedA = 0.; |
|
127 |
speedB = pwm_r; |
|
128 |
speedC = pwm_l; |
|
129 |
tel |
|
130 |
|
|
131 |
|
|
132 |
|
|
105 | 133 |
(* |
106 | 134 |
node automate(in1, in2, in3, in4, cntA, cntB, cntC, battery_voltage : int) |
107 | 135 |
returns (speedA, speedB, speedC : int); |
Also available in: Unified diff
Added function pure real