Revision 43dfd684
Added by PierreLoï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_reftheta)*k_i + 0. > pre(anti_windup*0.1)); 

121 


122 
err = errtheta + k_f_1*(x_11_refx_11) + k_f_2*(x_12_refx_12)+ k_f_3*(x_13_refx_13) +k_f_4*(x_14_refx_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