Project

General

Profile

« Previous | Next » 

Revision 0da3f528

Added by LĂ©lio Brun about 2 years ago

remove tests with include directives

View differences:

offline_tests/Fcn1.LUSTREC.lus
1
-- This file has been generated by CoCoSim2.
2

  
3
-- Compiler: Lustre compiler 2 (ToLustre.m)
4
-- Time: 07-Nov-2018 18:21:40
5
#open <lustrec_math>
6
node  bool_to_real(x : bool;)
7
returns(y : real;);
8
let
9
	y = if x then
10
		1.0
11
	    else 0.0;
12
tel
13

  
14

  
15
(*
16
Original block name: Fcn1
17
*)
18
node  Fcn1(In1_1 : real;
19
	In1_2 : real;
20
	In1_3 : real;
21
	In1_4 : real;
22
	In2_1 : real;
23
	In2_2 : real;
24
	In2_3 : real;
25
	In2_4 : real;
26
	In2_5 : real;
27
	In2_6 : real;
28
	In3_1 : real;
29
	In3_2 : real;
30
	In3_3 : real;
31
	In3_4 : real;
32
	In4_1 : real;
33
	In4_2 : real;
34
	In4_3 : real;
35
	In5_1 : real;
36
	In5_2 : real;
37
	In5_3 : real;
38
	In6_1 : real;
39
	In6_2 : real;
40
	In6_3 : real;
41
	In6_4 : real;
42
	In6_5 : real;
43
	In6_6 : real;)
44
returns(Out1_1 : real;
45
	Out2_1 : real;
46
	Out3_1 : real;
47
	Out4_1 : real;
48
	Out5_1 : real;
49
	Out6_1 : real;);
50
var Fcn_1 : real;
51
	Fcn1_1 : real;
52
	Fcn2_1 : real;
53
	Fcn3_1 : real;
54
	Fcn4_1 : real;
55
	Fcn5_1 : real;
56
	__time_step : real;
57
	__nb_step : int;
58
let
59
	Fcn_1 = 1.000000000000000 - 2.000000000000000 * ( In1_1 * In1_4 + In1_2 * In1_3 );
60
	Fcn1_1 = In2_2 * In2_6 - In2_5 * In2_3;
61
	Fcn2_1 = - In3_1 * In3_1 - In3_2 * In3_2 - In3_3 * In3_3 + In3_4 * In3_4;
62
	Fcn3_1 = cos(In4_3 / 2.000000000000000) * cos(In4_2 / 2.000000000000000) * sin(In4_1 / 2.000000000000000) - sin(In4_3 / 2.000000000000000) * sin(In4_2 / 2.000000000000000) * cos(In4_1 / 2.000000000000000);
63
	Fcn4_1 = sin(In5_1) * sin(In5_2) * cos(In5_3) - cos(In5_1) * sin(In5_3);
64
	Fcn5_1 = bool_to_real(In6_2 * In6_6 > In6_5 * In6_3);
65
	Out1_1 = Fcn_1;
66
	Out2_1 = Fcn1_1;
67
	Out3_1 = Fcn2_1;
68
	Out4_1 = Fcn3_1;
69
	Out5_1 = Fcn4_1;
70
	Out6_1 = Fcn5_1;
71
	__time_step = (0.0 -> ((pre __time_step) + 0.200000000000000));
72
	__nb_step = (0 -> ((pre __nb_step) + 1));
73
tel
74

  
offline_tests/Fcn2_PP.LUSTREC.lus
1
-- This file has been generated by CoCoSim2.
2

  
3
-- Compiler: Lustre compiler 2 (ToLustre.m)
4
-- Time: 03-Dec-2018 22:35:49
5
#open <lustrec_math>
6

  
7
(*
8
Original block name: Fcn2_PP
9
*)
10
node  Fcn2_PP(u_1 : real;
11
	u_2 : real;
12
	u_3 : real;
13
	u_4 : real;)
14
returns(Out1_1 : real;);
15
var theta_1 : real;
16
	__time_step : real;
17
	__nb_step : int;
18
let
19
	theta_1 = 2.000000000000000 * asin(sqrt(pow(u_2, 2.000000000000000) + pow(u_3, 2.000000000000000) + pow(u_4, 2.000000000000000)));
20
	Out1_1 = theta_1;
21
	__time_step = (0.0 -> ((pre __time_step) + 0.200000000000000));
22
	__nb_step = (0 -> ((pre __nb_step) + 1));
23
tel
24

  
offline_tests/GraphFun1.lus
1
-- This file has been generated by cocoSim
2

  
3
-- External Stateflow functions
4
#open <lustrec_math>
5

  
6
-- System nodes
7

  
8

  
9

  
10

  
11
node POINT__To__Chart_fJunction885_1_Condition_Action(x:real;
12
	y:real;
13
	z_1:real)
14

  
15
returns (z:real);
16

  
17

  
18
var 	z_2:real;
19

  
20

  
21
let
22

  
23

  
24

  
25
	z_2 
26
	= sqrt(x*x+y*y);
27
	
28

  
29
	(z) 
30
	= (z_2);
31
	
32

  
33
tel
34

  
35

  
36
--***************************************************State :Chart_f Automaton***************************************************
37

  
38
node Chart_f_node(idChart_f_1:int;
39
	x:real;
40
	y:real;
41
	z_1:real)
42

  
43
returns (idChart_f:int;
44
	z:real);
45

  
46

  
47
let
48

  
49
	 automaton chart_f
50

  
51
	state POINTChart_f:
52
	unless (idChart_f_1=0) restart POINT__TO__CHART_FJUNCTION885_1
53

  
54

  
55

  
56
	let
57

  
58
		(idChart_f, z) 
59
	= (idChart_f_1, z_1);
60
	
61

  
62
	tel
63

  
64

  
65

  
66
	state POINT__TO__CHART_FJUNCTION885_1:
67

  
68
	 var 	z_2:real;
69
	let
70

  
71
		-- transition trace :
72
	--POINT__To__Junction885_1
73
		-- condition Action : z=sqrt(x*x+y*y);
74
		
75
		(z_2) 
76
	= POINT__To__Chart_fJunction885_1_Condition_Action(x, y, z_1);
77
		
78

  
79
	(idChart_f, z) 
80
	=  (idChart_f_1, z_2);
81

  
82

  
83
	tel
84

  
85
	until true restart POINTChart_f
86

  
87

  
88

  
89
tel
90

  
91

  
92
--***************************************************State :Chart_f Automaton***************************************************
93

  
94
node f(x:real;
95
	y:real)
96

  
97
returns (z:real);
98

  
99

  
100
var z_1: real;
101

  
102
	idChart_f, idChart_f_1: int;
103

  
104
	let
105

  
106
	z_1 = 0.0 -> pre z;
107

  
108
	idChart_f_1 = 0 -> pre idChart_f;
109

  
110
	
111

  
112

  
113

  
114
	(idChart_f, z)
115
	 = Chart_f_node(idChart_f_1, x, y, z_1);
116

  
117

  
118
--unused outputs
119
	
120

  
121
tel
122

  
123

  
124

  
125

  
126

  
127

  
128

  
129
node Chart_B__To__Chart_A_1_Condition_Action(a_1:real;
130
	b_1:real;
131
	c_1:real)
132

  
133
returns (a:real;
134
	b:real;
135
	c:real);
136

  
137

  
138
var 	c_2:real;
139

  
140

  
141
let
142

  
143

  
144

  
145
	c_2 
146
	= f( a_1 , b_1 );
147
	
148

  
149
	(a, b, c) 
150
	= (a_1, b_1, c_2);
151
	
152

  
153
tel
154

  
155

  
156

  
157

  
158

  
159

  
160

  
161
node Chart_A__To__Chart_B_1_Condition_Action(a_1:real;
162
	b_1:real;
163
	c_1:real)
164

  
165
returns (a:real;
166
	b:real;
167
	c:real);
168

  
169

  
170
var 	c_2:real;
171

  
172

  
173
let
174

  
175

  
176

  
177
	c_2 
178
	= f( a_1 , b_1 );
179
	
180

  
181
	(a, b, c) 
182
	= (a_1, b_1, c_2);
183
	
184

  
185
tel
186

  
187

  
188

  
189

  
190

  
191

  
192
-- Entry action for state :Chart_B
193
node Chart_B_en(idChart_Chart_1:int;
194
	a_1:real;
195
	b_1:real;
196
	isInner:bool)
197

  
198
returns (idChart_Chart:int;
199
	a:real;
200
	b:real);
201

  
202

  
203
var 	idChart_Chart_2:int;
204
	a_2:real;
205
	b_2:real;
206

  
207

  
208
let
209

  
210

  
211

  
212
	-- set state as active 
213
	idChart_Chart_2 
214
	= 880;
215
	
216

  
217
	a_2 
218
	= if (not isInner) then 5.
219
	 else a_1;
220
	
221

  
222
	b_2 
223
	= if (not isInner) then 12.
224
	 else b_1;
225
	
226

  
227
	(idChart_Chart, a, b) 
228
	= (idChart_Chart_2, a_2, b_2);
229
	
230

  
231
tel
232

  
233

  
234

  
235

  
236

  
237
-- Exit action for state :Chart_B
238
node Chart_B_ex(idChart_Chart_1:int;
239
	isInner:bool)
240

  
241
returns (idChart_Chart:int);
242

  
243

  
244
var 	idChart_Chart_2:int;
245

  
246

  
247
let
248

  
249

  
250

  
251
	-- set state as inactive 
252
	idChart_Chart_2
253
	 = if (not isInner) then 0 else idChart_Chart_1;
254

  
255

  
256
	(idChart_Chart) 
257
	= (idChart_Chart_1);
258
	
259

  
260
tel
261

  
262

  
263

  
264

  
265

  
266

  
267
-- Entry action for state :Chart_A
268
node Chart_A_en(idChart_Chart_1:int;
269
	a_1:real;
270
	b_1:real;
271
	isInner:bool)
272

  
273
returns (idChart_Chart:int;
274
	a:real;
275
	b:real);
276

  
277

  
278
var 	idChart_Chart_2:int;
279
	a_2:real;
280
	b_2:real;
281

  
282

  
283
let
284

  
285

  
286

  
287
	-- set state as active 
288
	idChart_Chart_2 
289
	= 879;
290
	
291

  
292
	a_2 
293
	= if (not isInner) then 3.
294
	 else a_1;
295
	
296

  
297
	b_2 
298
	= if (not isInner) then 4.
299
	 else b_1;
300
	
301

  
302
	(idChart_Chart, a, b) 
303
	= (idChart_Chart_2, a_2, b_2);
304
	
305

  
306
tel
307

  
308

  
309

  
310

  
311

  
312
-- Exit action for state :Chart_A
313
node Chart_A_ex(idChart_Chart_1:int;
314
	isInner:bool)
315

  
316
returns (idChart_Chart:int);
317

  
318

  
319
var 	idChart_Chart_2:int;
320

  
321

  
322
let
323

  
324

  
325

  
326
	-- set state as inactive 
327
	idChart_Chart_2
328
	 = if (not isInner) then 0 else idChart_Chart_1;
329

  
330

  
331
	(idChart_Chart) 
332
	= (idChart_Chart_1);
333
	
334

  
335
tel
336

  
337

  
338
--***************************************************State :Chart_Chart Automaton***************************************************
339

  
340
node Chart_Chart_node(idChart_Chart_1:int;
341
	a_1:real;
342
	b_1:real;
343
	c_1:real)
344

  
345
returns (idChart_Chart:int;
346
	a:real;
347
	b:real;
348
	c:real);
349

  
350

  
351
let
352

  
353
	 automaton chart_chart
354

  
355
	state POINTChart_Chart:
356
	unless (idChart_Chart_1=0) restart POINT__TO__CHART_A_1
357

  
358

  
359

  
360
	unless (idChart_Chart_1=879) restart CHART_A__TO__CHART_B_1
361

  
362

  
363

  
364
	unless (idChart_Chart_1=880) restart CHART_B__TO__CHART_A_1
365

  
366

  
367

  
368
	unless (idChart_Chart_1=879) restart CHART_A_IDL
369

  
370
	unless (idChart_Chart_1=880) restart CHART_B_IDL
371

  
372
	let
373

  
374
		(idChart_Chart, a, b, c) 
375
	= (idChart_Chart_1, a_1, b_1, c_1);
376
	
377

  
378
	tel
379

  
380

  
381

  
382
	state POINT__TO__CHART_A_1:
383

  
384
	 var 	idChart_Chart_2:int;
385
	a_2:real;
386
	b_2:real;
387
	let
388

  
389
		-- transition trace :
390
	--POINT__To__Chart_A_1
391
		(idChart_Chart_2, a_2, b_2) 
392
	= Chart_A_en(idChart_Chart_1, a_1, b_1, false);
393
		
394

  
395
	(idChart_Chart, a, b) 
396
	=  (idChart_Chart_2, a_2, b_2);
397

  
398
	--add unused variables
399
	(c) 
400
	= (c_1);
401
	
402

  
403
	tel
404

  
405
	until true restart POINTChart_Chart
406

  
407

  
408

  
409
	state CHART_A__TO__CHART_B_1:
410

  
411
	 var 	idChart_Chart_2, idChart_Chart_3:int;
412
	a_2, a_3:real;
413
	b_2, b_3:real;
414
	c_2:real;
415
	let
416

  
417
		-- transition trace :
418
	--Chart_A__To__Chart_B_1
419
		-- condition Action : c=f(a,b)
420
		
421
		(a_2, b_2, c_2) 
422
	= Chart_A__To__Chart_B_1_Condition_Action(a_1, b_1, c_1);
423
		
424

  
425
		(idChart_Chart_2) 
426
	= Chart_A_ex(idChart_Chart_1, false);
427
		
428

  
429
		(idChart_Chart_3, a_3, b_3) 
430
	= Chart_B_en(idChart_Chart_2, a_2, b_2, false);
431
		
432

  
433
	(idChart_Chart, a, b, c) 
434
	=  (idChart_Chart_3, a_3, b_3, c_2);
435

  
436

  
437
	tel
438

  
439
	until true restart POINTChart_Chart
440

  
441

  
442

  
443
	state CHART_B__TO__CHART_A_1:
444

  
445
	 var 	idChart_Chart_2, idChart_Chart_3:int;
446
	a_2, a_3:real;
447
	b_2, b_3:real;
448
	c_2:real;
449
	let
450

  
451
		-- transition trace :
452
	--Chart_B__To__Chart_A_1
453
		-- condition Action : c=f(a,b)
454
		
455
		(a_2, b_2, c_2) 
456
	= Chart_B__To__Chart_A_1_Condition_Action(a_1, b_1, c_1);
457
		
458

  
459
		(idChart_Chart_2) 
460
	= Chart_B_ex(idChart_Chart_1, false);
461
		
462

  
463
		(idChart_Chart_3, a_3, b_3) 
464
	= Chart_A_en(idChart_Chart_2, a_2, b_2, false);
465
		
466

  
467
	(idChart_Chart, a, b, c) 
468
	=  (idChart_Chart_3, a_3, b_3, c_2);
469

  
470

  
471
	tel
472

  
473
	until true restart POINTChart_Chart
474

  
475

  
476

  
477
	state CHART_A_IDL:
478

  
479
	 	let
480

  
481
		
482

  
483
	(idChart_Chart, a, b, c) 
484
	= (idChart_Chart_1, a_1, b_1, c_1);
485
	
486

  
487
	tel
488

  
489
	until true restart POINTChart_Chart
490

  
491

  
492

  
493
	state CHART_B_IDL:
494

  
495
	 	let
496

  
497
		
498

  
499
	(idChart_Chart, a, b, c) 
500
	= (idChart_Chart_1, a_1, b_1, c_1);
501
	
502

  
503
	tel
504

  
505
	until true restart POINTChart_Chart
506

  
507

  
508

  
509
tel
510

  
511

  
512
--***************************************************State :Chart_Chart Automaton***************************************************
513

  
514
node GraphFun1_Chart(noInput :bool)
515

  
516
returns (c:real);
517

  
518

  
519
var c_1: real;
520

  
521
	a, a_1: real;
522

  
523
	b, b_1: real;
524

  
525
	idChart_Chart, idChart_Chart_1: int;
526

  
527
	let
528

  
529
	c_1 = 0.0 -> pre c;
530

  
531
	a_1 = 0.0 -> pre a;
532

  
533
	b_1 = 0.0 -> pre b;
534

  
535
	idChart_Chart_1 = 0 -> pre idChart_Chart;
536

  
537
	
538

  
539

  
540

  
541
	(idChart_Chart, a, b, c)
542
	 = Chart_Chart_node(idChart_Chart_1, a_1, b_1, c_1);
543

  
544

  
545
--unused outputs
546
	
547

  
548
tel
549

  
550

  
551

  
552
node GraphFun1 (i_virtual : real)
553
returns (c_1_1 : real); 
554
var
555
	Chart_1_1 : real;
556
	i_virtual_local : real;
557
let 
558
	Chart_1_1 =  GraphFun1_Chart(true);
559
	c_1_1 = Chart_1_1;
560
	i_virtual_local= 0.0 -> 1.0;
561
tel
562

  
offline_tests/Trigonometry_PP.LUSTREC.lus
1
-- This file has been generated by CoCoSim2.
2

  
3
-- Compiler: Lustre compiler 2 (ToLustre.m)
4
-- Time: 03-Dec-2018 23:08:32
5
#open <lustrec_math>
6

  
7
(*
8
Original block name: Trigonometry_PP
9
*)
10
node  Trigonometry_PP(_virtual : bool;)
11
returns(Out1_1 : real;
12
	Out2_1 : real;
13
	Out3_1 : real;
14
	Out4_1 : real;
15
	Out5_1 : real;
16
	Out6_1 : real;
17
	Out7_1 : real;
18
	Out8_1 : real;
19
	Out10_1 : real;
20
	Out11_1 : real;
21
	Out9_1 : real;
22
	Out12_1 : real;
23
	Out13_1 : real;);
24
var Constant_1 : real;
25
	Constant1_1 : real;
26
	TrigonometricFunction_1 : real;
27
	TrigonometricFunction1_1 : real;
28
	TrigonometricFunction10_1 : real;
29
	TrigonometricFunction11_1 : real;
30
	TrigonometricFunction11_2 : real;
31
	TrigonometricFunction2_1 : real;
32
	TrigonometricFunction3_1 : real;
33
	TrigonometricFunction4_1 : real;
34
	TrigonometricFunction5_1 : real;
35
	TrigonometricFunction6_1 : real;
36
	TrigonometricFunction7_1 : real;
37
	TrigonometricFunction8_1 : real;
38
	TrigonometricFunction9_1 : real;
39
	__time_step : real;
40
	__nb_step : int;
41
let
42
	Constant_1 = 0.150000000000000;
43
	Constant1_1 = 15.000000000000000;
44
	TrigonometricFunction_1 = sin(Constant_1);
45
	TrigonometricFunction1_1 = cos(Constant_1);
46
	TrigonometricFunction10_1 = acosh(Constant1_1);
47
	TrigonometricFunction11_1 = sin(Constant_1);
48
	TrigonometricFunction11_2 = cos(Constant_1);
49
	TrigonometricFunction2_1 = tan(Constant_1);
50
	TrigonometricFunction3_1 = asin(Constant_1);
51
	TrigonometricFunction4_1 = acos(Constant_1);
52
	TrigonometricFunction5_1 = atan(Constant_1);
53
	TrigonometricFunction6_1 = sinh(Constant_1);
54
	TrigonometricFunction7_1 = cosh(Constant_1);
55
	TrigonometricFunction8_1 = atanh(Constant_1);
56
	TrigonometricFunction9_1 = asinh(Constant1_1);
57
	Out1_1 = TrigonometricFunction_1;
58
	Out2_1 = TrigonometricFunction1_1;
59
	Out3_1 = TrigonometricFunction2_1;
60
	Out4_1 = TrigonometricFunction3_1;
61
	Out5_1 = TrigonometricFunction4_1;
62
	Out6_1 = TrigonometricFunction5_1;
63
	Out7_1 = TrigonometricFunction6_1;
64
	Out8_1 = TrigonometricFunction7_1;
65
	Out10_1 = TrigonometricFunction9_1;
66
	Out11_1 = TrigonometricFunction10_1;
67
	Out9_1 = TrigonometricFunction8_1;
68
	Out12_1 = TrigonometricFunction11_1;
69
	Out13_1 = TrigonometricFunction11_2;
70
	__time_step = (0.0 -> ((pre __time_step) + 0.200000000000000));
71
	__nb_step = (0 -> ((pre __nb_step) + 1));
72
tel
73

  
offline_tests/lego_anti_windup.lus
1
#open <conv> 
2
const k_f_1 = -0.870303;
3
const k_f_2 = -31.9978;
4
const k_f_3 = -1.1566;
5
const k_f_4 = -2.78873;
6

  
7
const k_i = -0.44721;   (* servo control integral gain *)
8
const k_phidot = 25.0;  (* turn target speed gain *)
9
const k_thetadot = 7.5; (* forward target speed gain *)
10

  
11
const battery_gain = 0.001089;
12
const battery_offset = 0.625;
13

  
14
const cmd_max = 100.;
15
const deg2rad = 0.01745329238;
16
const exec_period = 0.00400000019;
17

  
18
const a_d = 0.8; 
19
const a_r = 0.996;   
20

  
21

  
22

  
23
node discrete_integrator(exec_period, in : real)
24
returns (out: real);
25
let
26
  out = 0. -> pre (exec_period * in + out);
27
tel
28

  
29
node low_pass_filter(a, in: real) 
30
returns (out : real);
31
let 
32
  out = (1.-a)*in -> (1.-a)*in + (pre out) * a;
33
tel
34

  
35
node sat(min, max, in : real)
36
returns (out: real);
37
let
38
  out = if in <= min then min else (if in>= max then max else in);
39
tel
40

  
41
node cal_reference(cmd_forward, cmd_turn : real)
42
returns (pwm_turn, theta_ref, x_11_ref,x_12_ref,x_13_ref,x_14_ref : real);
43
var theta_dot_ref: real;
44
let
45
  theta_dot_ref = low_pass_filter(a_r,k_thetadot*cmd_forward/cmd_max);
46
  theta_ref=x_11_ref;
47
  x_11_ref = discrete_integrator(exec_period,theta_dot_ref);
48
  x_12_ref = 0.;
49
  x_13_ref = theta_dot_ref;
50
  x_14_ref = 0.;
51
  pwm_turn = cmd_turn*k_phidot/cmd_max;
52
tel
53

  
54
node cal_x1 (gyro, gyro_offset, theta_m_l, theta_m_r:real)
55
returns(theta, x_11,x_12,x_13,x_14 : real);
56
var psidot, psi, theta_l, theta_r: real;
57
let
58
  psidot = (gyro - gyro_offset)*deg2rad;
59
  psi = discrete_integrator(exec_period,psidot);
60
  theta_l= theta_m_l*deg2rad+ psi;
61
  theta_r = theta_m_r*deg2rad + psi;
62
  theta = (theta_l+theta_r)/2.;
63
  x_11 = theta;
64
  x_12 = psi;
65
  x_13 = (low_pass_filter(a_d,theta) - (0. -> pre(low_pass_filter(a_d,theta))))/exec_period;
66
  x_14 = psidot;
67
tel;
68

  
69
node cal_pwm(theta_m_l, theta_m_r, pwm_turn, err, battery : real)
70
returns(pwm_l, pwm_r,  anti_windup : real);
71
var fwd, theta_diff : real;
72
let
73
fwd = cmd_max * (err/(battery*battery_gain-battery_offset));
74
pwm_l = sat(-100.,100.,fwd+pwm_turn);
75
theta_diff = theta_m_l - theta_m_r;
76
pwm_r = sat(-100.,100.,fwd-pwm_turn+12.0*theta_diff);
77
anti_windup=  pwm_l - fwd+pwm_turn;
78
tel;
79

  
80
node lego_anti_windup (in1, in2, in3, in4, cntA, cntB, cntC, battery_voltage : int; gyro_offset :  real)
81
returns (speedA, speedB, speedC : int);
82
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;
83
let
84
 cmd_forward = if in2 <= 25 then -100. else 0.;
85
 cmd_turn = 0.;
86
 gyro = int_to_real(in3);
87
 theta_m_l = int_to_real(cntC);
88
 theta_m_r = int_to_real(cntB);
89

  
90
 (pwm_turn, theta_ref, x_11_ref,  x_12_ref, x_13_ref, x_14_ref) = cal_reference(cmd_forward, cmd_turn);
91

  
92
 (theta, x_11,  x_12, x_13, x_14) = cal_x1(gyro, gyro_offset, theta_m_l, theta_m_r);
93
 
94
 errtheta = discrete_integrator(exec_period, (theta_ref-theta)*k_i + 0. -> pre(anti_windup*0.1));
95
 
96
 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);
97
 
98
 (pwm_l,pwm_r,  anti_windup) = cal_pwm(theta_m_l, theta_m_r, pwm_turn, err, int_to_real(battery_voltage));
99

  
100
  speedA = 0;
101
  speedB = real_to_int(pwm_r);
102
  speedC = real_to_int(pwm_l);
103
tel
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

  
133
(*
134
node automate(in1, in2, in3, in4, cntA, cntB, cntC, battery_voltage : int)
135
returns (speedA, speedB, speedC : int);
136
var 
137
  cnt :  int;  gyro_offset :  real;
138
let
139
  cnt = 1 -> pre(cnt) +1;
140
  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);
141
  (speedA, speedB, speedC) = if cnt <= 250 then (0,  0,  0) else top(in1, in2, in3, in4, cntA, cntB, cntC, battery_voltage,  gyro_offset);
142
tel
143
*)
offline_tests/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 lego_lqr_pur (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
*)
offline_tests/math_2_test.lus
1
-- This file has been generated by cocoSim
2

  
3
#open <lustrec_math>
4

  
5
-- System nodes
6
node math_2_test (In6_1_1 : real; In7_1_1 : real; In8_1_1 : real; In9_1_1 : real; In11_1_1 : real; )
7
returns (Out6_1_1 : real;
8
	Out7_2_1 : real;
9
	Out8_3_1 : real;
10
	Out10_4_1 : real); 
11
var
12
	MathFunction5_1_1 : real;
13
	MathFunction6_1_1 : real;
14
	MathFunction7_1_1 : real;
15
	MathFunction9_1_1 : real;
16
	i_virtual_local : real;
17
let 
18
	MathFunction5_1_1 = In6_1_1 * In6_1_1;
19
	MathFunction6_1_1 = sqrt(In7_1_1);
20
	MathFunction7_1_1 = pow(In8_1_1, In9_1_1);
21
	MathFunction9_1_1 = 1.0 / In11_1_1;
22
	Out6_1_1 = MathFunction5_1_1;
23
	Out7_2_1 = MathFunction6_1_1;
24
	Out8_3_1 = MathFunction7_1_1;
25
	Out10_4_1 = MathFunction9_1_1;
26
	i_virtual_local= 0.0 -> 1.0;
27
tel
28

  
offline_tests/math_vect_2_test.lus
1
-- This file has been generated by cocoSim
2

  
3
#open <lustrec_math>
4

  
5
-- System nodes
6
node math_vect_2_test (In6_1_1 : real; In6_1_2 : real; In6_1_3 : real; In7_1_1 : real; In7_1_2 : real; In7_1_3 : real; In8_1_1 : real; In8_1_2 : real; In8_1_3 : real; In9_1_1 : real; In9_1_2 : real; In9_1_3 : real; In11_1_1 : real; In11_1_2 : real; In11_1_3 : real; In1_1_1 : real; In1_1_2 : real; In1_1_3 : real; In2_1_1 : real; In3_1_1 : real; In4_1_1 : real; In4_1_2 : real; In4_1_3 : real; )
7
returns (Out6_1_1 : real; Out6_1_2 : real; Out6_1_3 : real;
8
	Out7_2_1 : real; Out7_2_2 : real; Out7_2_3 : real;
9
	Out8_3_1 : real; Out8_3_2 : real; Out8_3_3 : real;
10
	Out10_4_1 : real; Out10_4_2 : real; Out10_4_3 : real;
11
	Out1_5_1 : real; Out1_5_2 : real; Out1_5_3 : real;
12
	Out2_6_1 : real; Out2_6_2 : real; Out2_6_3 : real); 
13
var
14
	MathFunction1_1_1 : real; MathFunction1_1_2 : real; MathFunction1_1_3 : real;
15
	MathFunction2_1_1 : real; MathFunction2_1_2 : real; MathFunction2_1_3 : real;
16
	MathFunction5_1_1 : real; MathFunction5_1_2 : real; MathFunction5_1_3 : real;
17
	MathFunction6_1_1 : real; MathFunction6_1_2 : real; MathFunction6_1_3 : real;
18
	MathFunction7_1_1 : real; MathFunction7_1_2 : real; MathFunction7_1_3 : real;
19
	MathFunction9_1_1 : real; MathFunction9_1_2 : real; MathFunction9_1_3 : real;
20
	i_virtual_local : real;
21
let 
22
	MathFunction1_1_1 = pow(In1_1_1, In2_1_1);
23
	MathFunction1_1_2 = pow(In1_1_2, In2_1_1);
24
	MathFunction1_1_3 = pow(In1_1_3, In2_1_1);
25
	MathFunction2_1_1 = pow(In3_1_1, In4_1_1);
26
	MathFunction2_1_2 = pow(In3_1_1, In4_1_2);
27
	MathFunction2_1_3 = pow(In3_1_1, In4_1_3);
28
	MathFunction5_1_1 = In6_1_1 * In6_1_1;
29
	MathFunction5_1_2 = In6_1_2 * In6_1_2;
30
	MathFunction5_1_3 = In6_1_3 * In6_1_3;
31
	MathFunction6_1_1 = sqrt(In7_1_1);
32
	MathFunction6_1_2 = sqrt(In7_1_2);
33
	MathFunction6_1_3 = sqrt(In7_1_3);
34
	MathFunction7_1_1 = pow(In8_1_1, In9_1_1);
35
	MathFunction7_1_2 = pow(In8_1_2, In9_1_2);
36
	MathFunction7_1_3 = pow(In8_1_3, In9_1_3);
37
	MathFunction9_1_1 = 1.0 / In11_1_1;
38
	MathFunction9_1_2 = 1.0 / In11_1_2;
39
	MathFunction9_1_3 = 1.0 / In11_1_3;
40
	Out6_1_1 = MathFunction5_1_1;
41
	Out6_1_2 = MathFunction5_1_2;
42
	Out6_1_3 = MathFunction5_1_3;
43
	Out7_2_1 = MathFunction6_1_1;
44
	Out7_2_2 = MathFunction6_1_2;
45
	Out7_2_3 = MathFunction6_1_3;
46
	Out8_3_1 = MathFunction7_1_1;
47
	Out8_3_2 = MathFunction7_1_2;
48
	Out8_3_3 = MathFunction7_1_3;
49
	Out10_4_1 = MathFunction9_1_1;
50
	Out10_4_2 = MathFunction9_1_2;
51
	Out10_4_3 = MathFunction9_1_3;
52
	Out1_5_1 = MathFunction1_1_1;
53
	Out1_5_2 = MathFunction1_1_2;
54
	Out1_5_3 = MathFunction1_1_3;
55
	Out2_6_1 = MathFunction2_1_1;
56
	Out2_6_2 = MathFunction2_1_2;
57
	Out2_6_3 = MathFunction2_1_3;
58
	i_virtual_local= 0.0 -> 1.0;
59
tel
60

  
offline_tests/trigo_mix_test_PP.LUSTREC.lus
1
-- This file has been generated by CoCoSim2.
2

  
3
-- Compiler: Lustre compiler 2 (ToLustre.m)
4
-- Time: 03-Dec-2018 22:14:40
5
#open <lustrec_math>
6

  
7
(*
8
Original block name: trigo_mix_test_PP
9
*)
10
node  trigo_mix_test_PP(In7_1 : real;
11
	In15_1 : real;
12
	In15_2 : real;
13
	In1_1 : real;
14
	In1_2 : real;
15
	In2_1 : real;
16
	In3_1 : real;
17
	In3_2 : real;
18
	In3_3 : real;
19
	In3_4 : real;
20
	In3_5 : real;
21
	In3_6 : real;
22
	In4_1 : real;
23
	In5_1 : real;
24
	In6_1 : real;
25
	In6_2 : real;
26
	In6_3 : real;
27
	In6_4 : real;
28
	In6_5 : real;
29
	In6_6 : real;)
30
returns(Out7_1 : real;
31
	Out7_2 : real;
32
	Out1_1 : real;
33
	Out1_2 : real;
34
	Out2_1 : real;
35
	Out2_2 : real;
36
	Out2_3 : real;
37
	Out2_4 : real;
38
	Out2_5 : real;
39
	Out2_6 : real;
40
	Out3_1 : real;
41
	Out3_2 : real;
42
	Out3_3 : real;
43
	Out3_4 : real;
44
	Out3_5 : real;
45
	Out3_6 : real;);
46
var TrigonometricFunction1_1 : real;
47
	TrigonometricFunction1_2 : real;
48
	TrigonometricFunction2_1 : real;
49
	TrigonometricFunction2_2 : real;
50
	TrigonometricFunction2_3 : real;
51
	TrigonometricFunction2_4 : real;
52
	TrigonometricFunction2_5 : real;
53
	TrigonometricFunction2_6 : real;
54
	TrigonometricFunction3_1 : real;
55
	TrigonometricFunction3_2 : real;
56
	TrigonometricFunction3_3 : real;
57
	TrigonometricFunction3_4 : real;
58
	TrigonometricFunction3_5 : real;
59
	TrigonometricFunction3_6 : real;
60
	TrigonometricFunction6_1 : real;
61
	TrigonometricFunction6_2 : real;
62
	__time_step : real;
63
	__nb_step : int;
64
let
65
	TrigonometricFunction1_1 = atan2(In1_1, In2_1);
66
	TrigonometricFunction1_2 = atan2(In1_2, In2_1);
67
	TrigonometricFunction2_1 = atan2(In3_1, In4_1);
68
	TrigonometricFunction2_2 = atan2(In3_2, In4_1);
69
	TrigonometricFunction2_3 = atan2(In3_3, In4_1);
70
	TrigonometricFunction2_4 = atan2(In3_4, In4_1);
71
	TrigonometricFunction2_5 = atan2(In3_5, In4_1);
72
	TrigonometricFunction2_6 = atan2(In3_6, In4_1);
73
	TrigonometricFunction3_1 = atan2(In5_1, In6_1);
74
	TrigonometricFunction3_2 = atan2(In5_1, In6_2);
75
	TrigonometricFunction3_3 = atan2(In5_1, In6_3);
76
	TrigonometricFunction3_4 = atan2(In5_1, In6_4);
77
	TrigonometricFunction3_5 = atan2(In5_1, In6_5);
78
	TrigonometricFunction3_6 = atan2(In5_1, In6_6);
79
	TrigonometricFunction6_1 = atan2(In7_1, In15_1);
80
	TrigonometricFunction6_2 = atan2(In7_1, In15_2);
81
	Out7_1 = TrigonometricFunction6_1;
82
	Out7_2 = TrigonometricFunction6_2;
83
	Out1_1 = TrigonometricFunction1_1;
84
	Out1_2 = TrigonometricFunction1_2;
85
	Out2_1 = TrigonometricFunction2_1;
86
	Out2_2 = TrigonometricFunction2_2;
87
	Out2_3 = TrigonometricFunction2_3;
88
	Out2_4 = TrigonometricFunction2_4;
89
	Out2_5 = TrigonometricFunction2_5;
90
	Out2_6 = TrigonometricFunction2_6;
91
	Out3_1 = TrigonometricFunction3_1;
92
	Out3_2 = TrigonometricFunction3_2;
93
	Out3_3 = TrigonometricFunction3_3;
94
	Out3_4 = TrigonometricFunction3_4;
95
	Out3_5 = TrigonometricFunction3_5;
96
	Out3_6 = TrigonometricFunction3_6;
97
	__time_step = (0.0 -> ((pre __time_step) + 0.200000000000000));
98
	__nb_step = (0 -> ((pre __nb_step) + 1));
99
tel
100

  

Also available in: Unified diff