Revision 7bc15356
Added by Pierre-Loïc Garoche about 9 years ago
test/src/lego_robot/controller/README | ||
---|---|---|
1 |
- lego_anti_windup.lus |
|
2 |
Original matlab controller translated into Lustre from Yorihisa Yamamoto |
|
3 |
+ antiwind-up and some tricks for wheels to prevent rotation from Romain Jobredeaux |
|
4 |
|
|
5 |
- lego_lqr_pur.lus |
|
6 |
Another controller from Romain Jobredeaux without any antiwind-up. |
test/src/lego_robot/controller/conv.c | ||
---|---|---|
1 |
int real_to_int (double in1, int *out) |
|
2 |
{ |
|
3 |
*out = (int)in1; |
|
4 |
|
|
5 |
return *out; |
|
6 |
} |
|
7 |
|
|
8 |
double int_to_real (int in1, double *out) |
|
9 |
{ |
|
10 |
*out = (double)in1; |
|
11 |
|
|
12 |
return *out; |
|
13 |
} |
|
14 |
|
|
15 |
|
test/src/lego_robot/controller/conv.h | ||
---|---|---|
1 |
int real_to_int (double in1, int *out); |
|
2 |
double int_to_real (int in1, double *out); |
|
3 |
|
test/src/lego_robot/controller/conv.lusi | ||
---|---|---|
1 |
node real_to_int(in1 : real) returns (out: int) stateless; |
|
2 |
|
|
3 |
node int_to_real(in1 : int) returns (out: real) stateless; |
test/src/lego_robot/controller/lego_anti_windup.lus | ||
---|---|---|
1 |
const k_f_1 = -0.870303; |
|
2 |
const k_f_2 = -31.9978; |
|
3 |
const k_f_3 = -1.1566; |
|
4 |
const k_f_4 = -2.78873; |
|
5 |
|
|
6 |
const k_i = -0.44721; (* servo control integral gain *) |
|
7 |
const k_phidot = 25.0; (* turn target speed gain *) |
|
8 |
const k_thetadot = 7.5; (* forward target speed gain *) |
|
9 |
|
|
10 |
const battery_gain = 0.001089; |
|
11 |
const battery_offset = 0.625; |
|
12 |
|
|
13 |
const cmd_max = 100.; |
|
14 |
const deg2rad = 0.01745329238; |
|
15 |
const exec_period = 0.00400000019; |
|
16 |
|
|
17 |
const a_d = 0.8; |
|
18 |
const a_r = 0.996; |
|
19 |
|
|
20 |
#open "conv" |
|
21 |
|
|
22 |
node discrete_integrator(exec_period, in : real) |
|
23 |
returns (out: real); |
|
24 |
let |
|
25 |
out = 0. -> pre (exec_period * in + out); |
|
26 |
tel |
|
27 |
|
|
28 |
node low_pass_filter(a, in: real) |
|
29 |
returns (out : real); |
|
30 |
let |
|
31 |
out = (1.-a)*in -> (1.-a)*in + (pre out) * a; |
|
32 |
tel |
|
33 |
|
|
34 |
node sat(min, max, in : real) |
|
35 |
returns (out: real); |
|
36 |
let |
|
37 |
out = if in <= min then min else (if in>= max then max else in); |
|
38 |
tel |
|
39 |
|
|
40 |
node cal_reference(cmd_forward, cmd_turn : real) |
|
41 |
returns (pwm_turn, theta_ref, x_11_ref,x_12_ref,x_13_ref,x_14_ref : real); |
|
42 |
var theta_dot_ref: real; |
|
43 |
let |
|
44 |
theta_dot_ref = low_pass_filter(a_r,k_thetadot*cmd_forward/cmd_max); |
|
45 |
theta_ref=x_11_ref; |
|
46 |
x_11_ref = discrete_integrator(exec_period,theta_dot_ref); |
|
47 |
x_12_ref = 0.; |
|
48 |
x_13_ref = theta_dot_ref; |
|
49 |
x_14_ref = 0.; |
|
50 |
pwm_turn = cmd_turn*k_phidot/cmd_max; |
|
51 |
tel |
|
52 |
|
|
53 |
node cal_x1 (gyro, gyro_offset, theta_m_l, theta_m_r:real) |
|
54 |
returns(theta, x_11,x_12,x_13,x_14 : real); |
|
55 |
var psidot, psi, theta_l, theta_r: real; |
|
56 |
let |
|
57 |
psidot = (gyro - gyro_offset)*deg2rad; |
|
58 |
psi = discrete_integrator(exec_period,psidot); |
|
59 |
theta_l= theta_m_l*deg2rad+ psi; |
|
60 |
theta_r = theta_m_r*deg2rad + psi; |
|
61 |
theta = (theta_l+theta_r)/2.; |
|
62 |
x_11 = theta; |
|
63 |
x_12 = psi; |
|
64 |
x_13 = (low_pass_filter(a_d,theta) - (0. -> pre(low_pass_filter(a_d,theta))))/exec_period; |
|
65 |
x_14 = psidot; |
|
66 |
tel; |
|
67 |
|
|
68 |
node cal_pwm(theta_m_l, theta_m_r, pwm_turn, err, battery : real) |
|
69 |
returns(pwm_l, pwm_r, anti_windup : real); |
|
70 |
var fwd, theta_diff : real; |
|
71 |
let |
|
72 |
fwd = cmd_max * (err/(battery*battery_gain-battery_offset)); |
|
73 |
pwm_l = sat(-100.,100.,fwd+pwm_turn); |
|
74 |
theta_diff = theta_m_l - theta_m_r; |
|
75 |
pwm_r = sat(-100.,100.,fwd-pwm_turn+12.0*theta_diff); |
|
76 |
anti_windup= pwm_l - fwd+pwm_turn; |
|
77 |
tel; |
|
78 |
|
|
79 |
node top (in1, in2, in3, in4, cntA, cntB, cntC, battery_voltage : int; gyro_offset : real) |
|
80 |
returns (speedA, speedB, speedC : int); |
|
81 |
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; |
|
82 |
let |
|
83 |
cmd_forward = if in2 <= 25 then -100. else 0.; |
|
84 |
cmd_turn = 0.; |
|
85 |
gyro = int_to_real(in3); |
|
86 |
theta_m_l = int_to_real(cntC); |
|
87 |
theta_m_r = int_to_real(cntB); |
|
88 |
|
|
89 |
(pwm_turn, theta_ref, x_11_ref, x_12_ref, x_13_ref, x_14_ref) = cal_reference(cmd_forward, cmd_turn); |
|
90 |
|
|
91 |
(theta, x_11, x_12, x_13, x_14) = cal_x1(gyro, gyro_offset, theta_m_l, theta_m_r); |
|
92 |
|
|
93 |
errtheta = discrete_integrator(exec_period, (theta_ref-theta)*k_i + 0. -> pre(anti_windup*0.1)); |
|
94 |
|
|
95 |
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); |
|
96 |
|
|
97 |
(pwm_l,pwm_r, anti_windup) = cal_pwm(theta_m_l, theta_m_r, pwm_turn, err, int_to_real(battery_voltage)); |
|
98 |
|
|
99 |
speedA = 0; |
|
100 |
speedB = real_to_int(pwm_r); |
|
101 |
speedC = real_to_int(pwm_l); |
|
102 |
tel |
|
103 |
|
|
104 |
(* |
|
105 |
node automate(in1, in2, in3, in4, cntA, cntB, cntC, battery_voltage : int) |
|
106 |
returns (speedA, speedB, speedC : int); |
|
107 |
var |
|
108 |
cnt : int; gyro_offset : real; |
|
109 |
let |
|
110 |
cnt = 1 -> pre(cnt) +1; |
|
111 |
gyro_offset = int_to_real(in3) -> int_to_real(in3)/int_to_real(cnt) + (int_to_real(cnt-1)/int_to_real(cnt))*pre(gyro_offset); |
|
112 |
(speedA, speedB, speedC) = if cnt <= 250 then (0, 0, 0) else top(in1, in2, in3, in4, cntA, cntB, cntC, battery_voltage, gyro_offset); |
|
113 |
tel |
|
114 |
*) |
test/src/lego_robot/controller/lego_lqr_pur.lus | ||
---|---|---|
1 |
#open "conv" |
|
2 |
|
|
3 |
const k_f_1 = -0.8373; |
|
4 |
const k_f_2 = -34.7621; |
|
5 |
const k_f_3 = 0.0224; |
|
6 |
const k_f_4 = -1.2977; |
|
7 |
const k_f_5 = -2.9648; |
|
8 |
const k_f_6 = 0.00051153; |
|
9 |
const k_f_7 = -0.4472; |
|
10 |
|
|
11 |
|
|
12 |
const k_phidot = 25.0; (* turn target speed gain *) |
|
13 |
const k_thetadot = 7.5; (* forward target speed gain *) |
|
14 |
|
|
15 |
const battery_gain = 0.001089; |
|
16 |
const battery_offset = 0.625; |
|
17 |
|
|
18 |
const cmd_max = 100.; |
|
19 |
const deg2rad = 0.01745329238; |
|
20 |
const exec_period = 0.00400000019; |
|
21 |
|
|
22 |
const a_d = 0.8; |
|
23 |
const a_r = 0.996; |
|
24 |
|
|
25 |
const W = 0.14; |
|
26 |
const R = 0.04; |
|
27 |
|
|
28 |
|
|
29 |
node discrete_integrator(exec_period, in : real) |
|
30 |
returns (out: real); |
|
31 |
let |
|
32 |
out = 0. -> pre (exec_period * in + out); |
|
33 |
tel |
|
34 |
|
|
35 |
node low_pass_filter(a, in: real) |
|
36 |
returns (out : real); |
|
37 |
let |
|
38 |
out = (1.-a)*in -> (1.-a)*in + (pre out) * a; |
|
39 |
tel |
|
40 |
|
|
41 |
node sat(min, max, in : real) |
|
42 |
returns (out: real); |
|
43 |
let |
|
44 |
out = if in <= min then min else (if in>= max then max else in); |
|
45 |
tel |
|
46 |
|
|
47 |
node cal_reference(cmd_forward, cmd_turn : real) |
|
48 |
returns (pwm_turn, x_11_ref,x_12_ref,x_13_ref,x_14_ref,x_15_ref,x_16_ref, x_17_ref : real); |
|
49 |
var theta_dot_ref: real; |
|
50 |
let |
|
51 |
theta_dot_ref = low_pass_filter(a_r,k_thetadot*cmd_forward/cmd_max); |
|
52 |
x_11_ref = discrete_integrator(exec_period,theta_dot_ref); |
|
53 |
x_12_ref = 0.; |
|
54 |
x_13_ref = 0.; |
|
55 |
x_14_ref = theta_dot_ref; |
|
56 |
x_15_ref = 0.; |
|
57 |
x_16_ref = 0.; |
|
58 |
x_17_ref = discrete_integrator(exec_period, x_11_ref) ; |
|
59 |
pwm_turn = cmd_turn*k_phidot/cmd_max; |
|
60 |
tel |
|
61 |
|
|
62 |
node cal_x1 (gyro, gyro_offset, theta_m_l, theta_m_r:real) |
|
63 |
returns(x_11,x_12,x_13,x_14,x_15,x_16, x_17 : real); |
|
64 |
var psidot, psi, theta_l, theta_r, phi, theta: real; |
|
65 |
let |
|
66 |
psidot = (gyro - gyro_offset)*deg2rad; |
|
67 |
psi = discrete_integrator(exec_period,psidot); |
|
68 |
theta_l= theta_m_l*deg2rad+ psi; |
|
69 |
theta_r = theta_m_r*deg2rad + psi; |
|
70 |
theta = (theta_l+theta_r)/2.; |
|
71 |
phi = R/W *(theta_r-theta_l); |
|
72 |
x_11 = theta; |
|
73 |
x_12 = psi; |
|
74 |
x_13 = phi; |
|
75 |
x_14 = (low_pass_filter(a_d,theta) - (0. -> pre(low_pass_filter(a_d,theta))))/exec_period; |
|
76 |
x_15 = psidot; |
|
77 |
x_16 = (low_pass_filter(a_d,phi) - (0. -> pre(low_pass_filter(a_d,phi))))/exec_period; |
|
78 |
x_17 = discrete_integrator(exec_period, (theta)); |
|
79 |
tel; |
|
80 |
|
|
81 |
node cal_pwm(theta_m_l, theta_m_r, pwm_turn, err_r, err_l, battery : real) |
|
82 |
returns(pwm_l, pwm_r : real); |
|
83 |
var fwd_l, fwd_r : real; |
|
84 |
let |
|
85 |
fwd_r = cmd_max * (err_r/(battery*battery_gain-battery_offset)); |
|
86 |
fwd_l = cmd_max * (err_l/(battery*battery_gain-battery_offset)); |
|
87 |
pwm_l = sat(-100.,100.,fwd_l+pwm_turn); |
|
88 |
pwm_r = sat(-100.,100.,fwd_r-pwm_turn); |
|
89 |
tel; |
|
90 |
|
|
91 |
node top (in1, in2, in3, in4, cntA, cntB, cntC, battery_voltage : int; gyro_offset : real) |
|
92 |
returns (speedA, speedB, speedC : int); |
|
93 |
var gyro, cmd_forward, cmd_turn, theta_m_l, theta_m_r, pwm_turn, x_11_ref, x_12_ref, x_13_ref, x_14_ref, x_15_ref, x_16_ref,x_17_ref, x_11, x_12, x_13, x_14,x_15,x_16,x_17, err_r, err_l, pwm_l, pwm_r: real; |
|
94 |
let |
|
95 |
cmd_forward = if in2 <= 25 then -100. else 0.; |
|
96 |
cmd_turn = 0.; |
|
97 |
gyro = int_to_real(in3); |
|
98 |
theta_m_l = int_to_real(cntC); |
|
99 |
theta_m_r = int_to_real(cntB); |
|
100 |
|
|
101 |
(pwm_turn, x_11_ref, x_12_ref, x_13_ref, x_14_ref,x_15_ref,x_16_ref, x_17_ref) = cal_reference(cmd_forward, cmd_turn); |
|
102 |
|
|
103 |
(x_11, x_12, x_13, x_14, x_15, x_16, x_17) = cal_x1(gyro, gyro_offset, theta_m_l, theta_m_r); |
|
104 |
|
|
105 |
err_r = 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)+k_f_5*(x_15_ref-x_15)+k_f_6*(x_16_ref-x_16) + k_f_7*(x_17_ref-x_17); |
|
106 |
|
|
107 |
err_l = 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)+k_f_5*(x_15_ref-x_15)-k_f_6*(x_16_ref-x_16) + k_f_7*(x_17_ref-x_17); |
|
108 |
|
|
109 |
(pwm_l,pwm_r) = cal_pwm(theta_m_l, theta_m_r, pwm_turn, err_r, err_l, int_to_real(battery_voltage)); |
|
110 |
|
|
111 |
speedA = 0; |
|
112 |
speedB = real_to_int(pwm_r); |
|
113 |
speedC = real_to_int(pwm_l); |
|
114 |
tel |
|
115 |
(* |
|
116 |
node automate(in1, in2, in3, in4, cntA, cntB, cntC, battery_voltage : int) |
|
117 |
returns (speedA, speedB, speedC : int); |
|
118 |
var |
|
119 |
cnt : int; gyro_offset : real; |
|
120 |
let |
|
121 |
cnt = 1 -> pre(cnt) +1; |
|
122 |
gyro_offset = int_to_real(in3) -> int_to_real(in3)/int_to_real(cnt) + (int_to_real(cnt-1)/int_to_real(cnt))*pre(gyro_offset); |
|
123 |
(speedA, speedB, speedC) = if cnt <= 250 then (0, 0, 0) else top(in1, in2, in3, in4, cntA, cntB, cntC, battery_voltage, gyro_offset); |
|
124 |
tel |
|
125 |
*) |
Also available in: Unified diff
lego robot example