Project

General

Profile

« Previous | Next » 

Revision 43dfd684

Added by Pierre-Loïc Garoche about 4 years ago

Added function pure real

View differences:

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