Project

General

Profile

« Previous | Next » 

Revision 7bc15356

Added by Pierre-Loïc Garoche almost 8 years ago

lego robot example

View differences:

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