CristalCaveGem » LustreC

-- This file has been generated by CoCoSim2.

-- Compiler: Lustre compiler 2 (ToLustre.m)

-- Time: 07-Nov-2018 18:21:40

#open <lustrec_math>

node  bool_to_real(x : bool;)

returns(y : real;);

let

	y = if x then

		1.0

	    else 0.0;

tel

(*

Original block name: Fcn1

*)

node  Fcn1(In1_1 : real;

	In1_2 : real;

	In1_3 : real;

	In1_4 : real;

	In2_1 : real;

	In2_2 : real;

	In2_3 : real;

	In2_4 : real;

	In2_5 : real;

	In2_6 : real;

	In3_1 : real;

	In3_2 : real;

	In3_3 : real;

	In3_4 : real;

	In4_1 : real;

	In4_2 : real;

	In4_3 : real;

	In5_1 : real;

	In5_2 : real;

	In5_3 : real;

	In6_1 : real;

	In6_2 : real;

	In6_3 : real;

	In6_4 : real;

	In6_5 : real;

	In6_6 : real;)

returns(Out1_1 : real;

	Out2_1 : real;

	Out3_1 : real;

	Out4_1 : real;

	Out5_1 : real;

	Out6_1 : real;);

var Fcn_1 : real;

	Fcn1_1 : real;

	Fcn2_1 : real;

	Fcn3_1 : real;

	Fcn4_1 : real;

	Fcn5_1 : real;

	__time_step : real;

	__nb_step : int;

let

	Fcn_1 = 1.000000000000000 - 2.000000000000000 * ( In1_1 * In1_4 + In1_2 * In1_3 );

	Fcn1_1 = In2_2 * In2_6 - In2_5 * In2_3;

	Fcn2_1 = - In3_1 * In3_1 - In3_2 * In3_2 - In3_3 * In3_3 + In3_4 * In3_4;

	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);

	Fcn4_1 = sin(In5_1) * sin(In5_2) * cos(In5_3) - cos(In5_1) * sin(In5_3);

	Fcn5_1 = bool_to_real(In6_2 * In6_6 > In6_5 * In6_3);

	Out1_1 = Fcn_1;

	Out2_1 = Fcn1_1;

	Out3_1 = Fcn2_1;

	Out4_1 = Fcn3_1;

	Out5_1 = Fcn4_1;

	Out6_1 = Fcn5_1;

	__time_step = (0.0 -> ((pre __time_step) + 0.200000000000000));

	__nb_step = (0 -> ((pre __nb_step) + 1));

tel

-- This file has been generated by CoCoSim2.

-- Compiler: Lustre compiler 2 (ToLustre.m)

-- Time: 03-Dec-2018 22:35:49

#open <lustrec_math>

(*

Original block name: Fcn2_PP

*)

node  Fcn2_PP(u_1 : real;

	u_2 : real;

	u_3 : real;

	u_4 : real;)

returns(Out1_1 : real;);

var theta_1 : real;

	__time_step : real;

	__nb_step : int;

let

	theta_1 = 2.000000000000000 * asin(sqrt(pow(u_2, 2.000000000000000) + pow(u_3, 2.000000000000000) + pow(u_4, 2.000000000000000)));

	Out1_1 = theta_1;

	__time_step = (0.0 -> ((pre __time_step) + 0.200000000000000));

	__nb_step = (0 -> ((pre __nb_step) + 1));

tel

-- This file has been generated by cocoSim

-- External Stateflow functions

#open <lustrec_math>

-- System nodes

node POINT__To__Chart_fJunction885_1_Condition_Action(x:real;

	y:real;

	z_1:real)

returns (z:real);

var 	z_2:real;

let

	z_2 

	= sqrt(x*x+y*y);

	(z) 

	= (z_2);

tel

--***************************************************State :Chart_f Automaton***************************************************

node Chart_f_node(idChart_f_1:int;

	x:real;

	y:real;

	z_1:real)

returns (idChart_f:int;

	z:real);

let

	 automaton chart_f

	state POINTChart_f:

	unless (idChart_f_1=0) restart POINT__TO__CHART_FJUNCTION885_1

	let

		(idChart_f, z) 

	= (idChart_f_1, z_1);

	tel

	state POINT__TO__CHART_FJUNCTION885_1:

	 var 	z_2:real;

	let

		-- transition trace :

	--POINT__To__Junction885_1

		-- condition Action : z=sqrt(x*x+y*y);

		(z_2) 

	= POINT__To__Chart_fJunction885_1_Condition_Action(x, y, z_1);

	(idChart_f, z) 

	=  (idChart_f_1, z_2);

	tel

	until true restart POINTChart_f

tel

--***************************************************State :Chart_f Automaton***************************************************

node f(x:real;

	y:real)

returns (z:real);

var z_1: real;

	idChart_f, idChart_f_1: int;

	let

	z_1 = 0.0 -> pre z;

	idChart_f_1 = 0 -> pre idChart_f;

	(idChart_f, z)

	 = Chart_f_node(idChart_f_1, x, y, z_1);

--unused outputs

tel

node Chart_B__To__Chart_A_1_Condition_Action(a_1:real;

	b_1:real;

	c_1:real)

returns (a:real;

	b:real;

	c:real);

var 	c_2:real;

let

	c_2 

	= f( a_1 , b_1 );

	(a, b, c) 

	= (a_1, b_1, c_2);

tel

node Chart_A__To__Chart_B_1_Condition_Action(a_1:real;

	b_1:real;

	c_1:real)

returns (a:real;

	b:real;

	c:real);

var 	c_2:real;

let

	c_2 

	= f( a_1 , b_1 );

	(a, b, c) 

	= (a_1, b_1, c_2);

tel

-- Entry action for state :Chart_B

node Chart_B_en(idChart_Chart_1:int;

	a_1:real;

	b_1:real;

	isInner:bool)

returns (idChart_Chart:int;

	a:real;

	b:real);

var 	idChart_Chart_2:int;

	a_2:real;

	b_2:real;

let

	-- set state as active 

	idChart_Chart_2 

	= 880;

	a_2 

	= if (not isInner) then 5.

	 else a_1;

	b_2 

	= if (not isInner) then 12.

	 else b_1;

	(idChart_Chart, a, b) 

	= (idChart_Chart_2, a_2, b_2);

tel

-- Exit action for state :Chart_B

node Chart_B_ex(idChart_Chart_1:int;

	isInner:bool)

returns (idChart_Chart:int);

var 	idChart_Chart_2:int;

let

	-- set state as inactive 

	idChart_Chart_2

	 = if (not isInner) then 0 else idChart_Chart_1;

	(idChart_Chart) 

	= (idChart_Chart_1);

tel

-- Entry action for state :Chart_A

node Chart_A_en(idChart_Chart_1:int;

	a_1:real;

	b_1:real;

	isInner:bool)

returns (idChart_Chart:int;

	a:real;

	b:real);

var 	idChart_Chart_2:int;

	a_2:real;

	b_2:real;

let

	-- set state as active 

	idChart_Chart_2 

	= 879;

	a_2 

	= if (not isInner) then 3.

	 else a_1;

	b_2 

	= if (not isInner) then 4.

	 else b_1;

	(idChart_Chart, a, b) 

	= (idChart_Chart_2, a_2, b_2);

tel

-- Exit action for state :Chart_A

node Chart_A_ex(idChart_Chart_1:int;

	isInner:bool)

returns (idChart_Chart:int);

var 	idChart_Chart_2:int;

let

	-- set state as inactive 

	idChart_Chart_2

	 = if (not isInner) then 0 else idChart_Chart_1;

	(idChart_Chart) 

	= (idChart_Chart_1);

tel

--***************************************************State :Chart_Chart Automaton***************************************************

node Chart_Chart_node(idChart_Chart_1:int;

	a_1:real;

	b_1:real;

	c_1:real)

returns (idChart_Chart:int;

	a:real;

	b:real;

	c:real);

let

	 automaton chart_chart

	state POINTChart_Chart:

	unless (idChart_Chart_1=0) restart POINT__TO__CHART_A_1

	unless (idChart_Chart_1=879) restart CHART_A__TO__CHART_B_1

	unless (idChart_Chart_1=880) restart CHART_B__TO__CHART_A_1

	unless (idChart_Chart_1=879) restart CHART_A_IDL

	unless (idChart_Chart_1=880) restart CHART_B_IDL

	let

		(idChart_Chart, a, b, c) 

	= (idChart_Chart_1, a_1, b_1, c_1);

	tel

	state POINT__TO__CHART_A_1:

	 var 	idChart_Chart_2:int;

	a_2:real;

	b_2:real;

	let

		-- transition trace :

	--POINT__To__Chart_A_1

		(idChart_Chart_2, a_2, b_2) 

	= Chart_A_en(idChart_Chart_1, a_1, b_1, false);

	(idChart_Chart, a, b) 

	=  (idChart_Chart_2, a_2, b_2);

	--add unused variables

	(c) 

	= (c_1);

	tel

	until true restart POINTChart_Chart

	state CHART_A__TO__CHART_B_1:

	 var 	idChart_Chart_2, idChart_Chart_3:int;

	a_2, a_3:real;

	b_2, b_3:real;

	c_2:real;

	let

		-- transition trace :

	--Chart_A__To__Chart_B_1

		-- condition Action : c=f(a,b)

		(a_2, b_2, c_2) 

	= Chart_A__To__Chart_B_1_Condition_Action(a_1, b_1, c_1);

		(idChart_Chart_2) 

	= Chart_A_ex(idChart_Chart_1, false);

		(idChart_Chart_3, a_3, b_3) 

	= Chart_B_en(idChart_Chart_2, a_2, b_2, false);

	(idChart_Chart, a, b, c) 

	=  (idChart_Chart_3, a_3, b_3, c_2);

	tel

	until true restart POINTChart_Chart

	state CHART_B__TO__CHART_A_1:

	 var 	idChart_Chart_2, idChart_Chart_3:int;

	a_2, a_3:real;

	b_2, b_3:real;

	c_2:real;

	let

		-- transition trace :

	--Chart_B__To__Chart_A_1

		-- condition Action : c=f(a,b)

		(a_2, b_2, c_2) 

	= Chart_B__To__Chart_A_1_Condition_Action(a_1, b_1, c_1);

		(idChart_Chart_2) 

	= Chart_B_ex(idChart_Chart_1, false);

		(idChart_Chart_3, a_3, b_3) 

	= Chart_A_en(idChart_Chart_2, a_2, b_2, false);

	(idChart_Chart, a, b, c) 

	=  (idChart_Chart_3, a_3, b_3, c_2);

	tel

	until true restart POINTChart_Chart

	state CHART_A_IDL:

	 	let

	(idChart_Chart, a, b, c) 

	= (idChart_Chart_1, a_1, b_1, c_1);

	tel

	until true restart POINTChart_Chart

	state CHART_B_IDL:

	 	let

	(idChart_Chart, a, b, c) 

	= (idChart_Chart_1, a_1, b_1, c_1);

	tel

	until true restart POINTChart_Chart

tel

--***************************************************State :Chart_Chart Automaton***************************************************

node GraphFun1_Chart(noInput :bool)

returns (c:real);

var c_1: real;

	a, a_1: real;

	b, b_1: real;

	idChart_Chart, idChart_Chart_1: int;

	let

	c_1 = 0.0 -> pre c;

	a_1 = 0.0 -> pre a;

	b_1 = 0.0 -> pre b;

	idChart_Chart_1 = 0 -> pre idChart_Chart;

	(idChart_Chart, a, b, c)

	 = Chart_Chart_node(idChart_Chart_1, a_1, b_1, c_1);

--unused outputs

tel

node GraphFun1 (i_virtual : real)

returns (c_1_1 : real); 

var

	Chart_1_1 : real;

	i_virtual_local : real;

let 

	Chart_1_1 =  GraphFun1_Chart(true);

	c_1_1 = Chart_1_1;

	i_virtual_local= 0.0 -> 1.0;

tel

-- This file has been generated by CoCoSim2.

-- Compiler: Lustre compiler 2 (ToLustre.m)

-- Time: 03-Dec-2018 23:08:32

#open <lustrec_math>

(*

Original block name: Trigonometry_PP

*)

node  Trigonometry_PP(_virtual : bool;)

returns(Out1_1 : real;

	Out2_1 : real;

	Out3_1 : real;

	Out4_1 : real;

	Out5_1 : real;

	Out6_1 : real;

	Out7_1 : real;

	Out8_1 : real;

	Out10_1 : real;

	Out11_1 : real;

	Out9_1 : real;

	Out12_1 : real;

	Out13_1 : real;);

var Constant_1 : real;

	Constant1_1 : real;

	TrigonometricFunction_1 : real;

	TrigonometricFunction1_1 : real;

	TrigonometricFunction10_1 : real;

	TrigonometricFunction11_1 : real;

	TrigonometricFunction11_2 : real;

	TrigonometricFunction2_1 : real;

	TrigonometricFunction3_1 : real;

	TrigonometricFunction4_1 : real;

	TrigonometricFunction5_1 : real;

	TrigonometricFunction6_1 : real;

	TrigonometricFunction7_1 : real;

	TrigonometricFunction8_1 : real;

	TrigonometricFunction9_1 : real;

	__time_step : real;

	__nb_step : int;

let

	Constant_1 = 0.150000000000000;

	Constant1_1 = 15.000000000000000;

	TrigonometricFunction_1 = sin(Constant_1);

	TrigonometricFunction1_1 = cos(Constant_1);

	TrigonometricFunction10_1 = acosh(Constant1_1);

	TrigonometricFunction11_1 = sin(Constant_1);

	TrigonometricFunction11_2 = cos(Constant_1);

	TrigonometricFunction2_1 = tan(Constant_1);

	TrigonometricFunction3_1 = asin(Constant_1);

	TrigonometricFunction4_1 = acos(Constant_1);

	TrigonometricFunction5_1 = atan(Constant_1);

	TrigonometricFunction6_1 = sinh(Constant_1);

	TrigonometricFunction7_1 = cosh(Constant_1);

	TrigonometricFunction8_1 = atanh(Constant_1);

	TrigonometricFunction9_1 = asinh(Constant1_1);

	Out1_1 = TrigonometricFunction_1;

	Out2_1 = TrigonometricFunction1_1;

	Out3_1 = TrigonometricFunction2_1;

	Out4_1 = TrigonometricFunction3_1;

	Out5_1 = TrigonometricFunction4_1;

	Out6_1 = TrigonometricFunction5_1;

	Out7_1 = TrigonometricFunction6_1;

	Out8_1 = TrigonometricFunction7_1;

	Out10_1 = TrigonometricFunction9_1;

	Out11_1 = TrigonometricFunction10_1;

	Out9_1 = TrigonometricFunction8_1;

	Out12_1 = TrigonometricFunction11_1;

	Out13_1 = TrigonometricFunction11_2;

	__time_step = (0.0 -> ((pre __time_step) + 0.200000000000000));

	__nb_step = (0 -> ((pre __nb_step) + 1));

tel

#open <conv> 

const k_f_1 = -0.870303;

const k_f_2 = -31.9978;

const k_f_3 = -1.1566;

const k_f_4 = -2.78873;

const k_i = -0.44721;   (* servo control integral gain *)

const k_phidot = 25.0;  (* turn target speed gain *)

const k_thetadot = 7.5; (* forward target speed gain *)

const battery_gain = 0.001089;

const battery_offset = 0.625;

const cmd_max = 100.;

const deg2rad = 0.01745329238;

const exec_period = 0.00400000019;

const a_d = 0.8; 

const a_r = 0.996;   

node discrete_integrator(exec_period, in : real)

returns (out: real);

let

  out = 0. -> pre (exec_period * in + out);

tel

node low_pass_filter(a, in: real) 

returns (out : real);

let 

  out = (1.-a)*in -> (1.-a)*in + (pre out) * a;

tel

node sat(min, max, in : real)

returns (out: real);

let

  out = if in <= min then min else (if in>= max then max else in);

tel

node cal_reference(cmd_forward, cmd_turn : real)

returns (pwm_turn, theta_ref, x_11_ref,x_12_ref,x_13_ref,x_14_ref : real);

var theta_dot_ref: real;

let

  theta_dot_ref = low_pass_filter(a_r,k_thetadot*cmd_forward/cmd_max);

  theta_ref=x_11_ref;

  x_11_ref = discrete_integrator(exec_period,theta_dot_ref);

  x_12_ref = 0.;

  x_13_ref = theta_dot_ref;

  x_14_ref = 0.;

  pwm_turn = cmd_turn*k_phidot/cmd_max;

tel

node cal_x1 (gyro, gyro_offset, theta_m_l, theta_m_r:real)

returns(theta, x_11,x_12,x_13,x_14 : real);

var psidot, psi, theta_l, theta_r: real;

let

  psidot = (gyro - gyro_offset)*deg2rad;

  psi = discrete_integrator(exec_period,psidot);

  theta_l= theta_m_l*deg2rad+ psi;

  theta_r = theta_m_r*deg2rad + psi;

  theta = (theta_l+theta_r)/2.;

  x_11 = theta;

  x_12 = psi;

  x_13 = (low_pass_filter(a_d,theta) - (0. -> pre(low_pass_filter(a_d,theta))))/exec_period;

  x_14 = psidot;

tel;

node cal_pwm(theta_m_l, theta_m_r, pwm_turn, err, battery : real)

returns(pwm_l, pwm_r,  anti_windup : real);

var fwd, theta_diff : real;

let

fwd = cmd_max * (err/(battery*battery_gain-battery_offset));

pwm_l = sat(-100.,100.,fwd+pwm_turn);

theta_diff = theta_m_l - theta_m_r;

pwm_r = sat(-100.,100.,fwd-pwm_turn+12.0*theta_diff);

anti_windup=  pwm_l - fwd+pwm_turn;

tel;

node lego_anti_windup (in1, in2, in3, in4, cntA, cntB, cntC, battery_voltage : int; gyro_offset :  real)

returns (speedA, speedB, speedC : int);

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;

let

 cmd_forward = if in2 <= 25 then -100. else 0.;

 cmd_turn = 0.;

 gyro = int_to_real(in3);

 theta_m_l = int_to_real(cntC);

 theta_m_r = int_to_real(cntB);

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

 (theta, x_11,  x_12, x_13, x_14) = cal_x1(gyro, gyro_offset, theta_m_l, theta_m_r);

 errtheta = discrete_integrator(exec_period, (theta_ref-theta)*k_i + 0. -> pre(anti_windup*0.1));

 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);

 (pwm_l,pwm_r,  anti_windup) = cal_pwm(theta_m_l, theta_m_r, pwm_turn, err, int_to_real(battery_voltage));

  speedA = 0;

  speedB = real_to_int(pwm_r);

  speedC = real_to_int(pwm_l);

tel

node lego_anti_windup_real (in1, in2, in3, in4, cntA, cntB, cntC, battery_voltage : real; gyro_offset :  real)

returns (speedA, speedB, speedC : real);

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;

let

 cmd_forward = if in2 <= 25. then -100. else 0.;

 cmd_turn = 0.;

 gyro = in3;

 theta_m_l = cntC;

 theta_m_r = cntB;

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

 (theta, x_11,  x_12, x_13, x_14) = cal_x1(gyro, gyro_offset, theta_m_l, theta_m_r);

 errtheta = discrete_integrator(exec_period, (theta_ref-theta)*k_i + 0. -> pre(anti_windup*0.1));

 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);

 (pwm_l,pwm_r,  anti_windup) = cal_pwm(theta_m_l, theta_m_r, pwm_turn, err, battery_voltage);

  speedA = 0.;

  speedB = pwm_r;

  speedC = pwm_l;

tel

(*

node automate(in1, in2, in3, in4, cntA, cntB, cntC, battery_voltage : int)

returns (speedA, speedB, speedC : int);

var 

  cnt :  int;  gyro_offset :  real;

let

  cnt = 1 -> pre(cnt) +1;

  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);

  (speedA, speedB, speedC) = if cnt <= 250 then (0,  0,  0) else top(in1, in2, in3, in4, cntA, cntB, cntC, battery_voltage,  gyro_offset);

tel

*)

#open <conv>

const k_f_1 = -0.8373;

const k_f_2 = -34.7621;

const k_f_3 = 0.0224;

const k_f_4 = -1.2977;

const k_f_5 = -2.9648;

const k_f_6 = 0.00051153;

const k_f_7 =  -0.4472;

const k_phidot = 25.0;  (* turn target speed gain *)

const k_thetadot = 7.5; (* forward target speed gain *)

const battery_gain = 0.001089;

const battery_offset = 0.625;

const cmd_max = 100.;

const deg2rad = 0.01745329238;

const exec_period = 0.00400000019;

const a_d = 0.8; 

const a_r = 0.996;   

const W = 0.14;

const R = 0.04;

node discrete_integrator(exec_period, in : real)

returns (out: real);

let

  out = 0. -> pre (exec_period * in + out);

tel

node low_pass_filter(a, in: real) 

returns (out : real);

let 

  out = (1.-a)*in -> (1.-a)*in + (pre out) * a;

tel

node sat(min, max, in : real)

returns (out: real);

let

  out = if in <= min then min else (if in>= max then max else in);

tel

node cal_reference(cmd_forward, cmd_turn : real)

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);

var theta_dot_ref: real;

let

  theta_dot_ref = low_pass_filter(a_r,k_thetadot*cmd_forward/cmd_max);

  x_11_ref = discrete_integrator(exec_period,theta_dot_ref);

  x_12_ref = 0.;

  x_13_ref = 0.;

  x_14_ref = theta_dot_ref;

  x_15_ref = 0.;

  x_16_ref = 0.;

  x_17_ref = discrete_integrator(exec_period, x_11_ref) ;

  pwm_turn = cmd_turn*k_phidot/cmd_max;

tel

node cal_x1 (gyro, gyro_offset, theta_m_l, theta_m_r:real)

returns(x_11,x_12,x_13,x_14,x_15,x_16, x_17 : real);

var psidot, psi, theta_l, theta_r, phi, theta: real;

let

  psidot = (gyro - gyro_offset)*deg2rad;

  psi = discrete_integrator(exec_period,psidot);

  theta_l= theta_m_l*deg2rad+ psi;

  theta_r = theta_m_r*deg2rad + psi;

  theta = (theta_l+theta_r)/2.;

  phi = R/W *(theta_r-theta_l);

  x_11 = theta;

  x_12 = psi;

  x_13 = phi;

  x_14 = (low_pass_filter(a_d,theta) - (0. -> pre(low_pass_filter(a_d,theta))))/exec_period;

  x_15 = psidot;

  x_16 = (low_pass_filter(a_d,phi) - (0. -> pre(low_pass_filter(a_d,phi))))/exec_period;

  x_17 = discrete_integrator(exec_period, (theta));

tel;

node cal_pwm(theta_m_l, theta_m_r, pwm_turn, err_r, err_l, battery : real)

returns(pwm_l, pwm_r : real);

var fwd_l, fwd_r : real;

let

fwd_r = cmd_max * (err_r/(battery*battery_gain-battery_offset));

fwd_l = cmd_max * (err_l/(battery*battery_gain-battery_offset));

pwm_l = sat(-100.,100.,fwd_l+pwm_turn);

pwm_r = sat(-100.,100.,fwd_r-pwm_turn);

tel;

node lego_lqr_pur (in1, in2, in3, in4, cntA, cntB, cntC, battery_voltage : int; gyro_offset :  real)

returns (speedA, speedB, speedC : int);

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;

let

 cmd_forward = if in2 <= 25 then -100. else 0.;

 cmd_turn = 0.;

 gyro = int_to_real(in3);

 theta_m_l = int_to_real(cntC);

 theta_m_r = int_to_real(cntB);

 (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);

 (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);

 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);

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);

 (pwm_l,pwm_r) = cal_pwm(theta_m_l, theta_m_r, pwm_turn, err_r, err_l, int_to_real(battery_voltage));

  speedA = 0;

  speedB = real_to_int(pwm_r);

  speedC = real_to_int(pwm_l);

tel

(*

node automate(in1, in2, in3, in4, cntA, cntB, cntC, battery_voltage : int)

returns (speedA, speedB, speedC : int);

var 

  cnt :  int;  gyro_offset :  real;

let

  cnt = 1 -> pre(cnt) +1;

  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);

  (speedA, speedB, speedC) = if cnt <= 250 then (0,  0,  0) else top(in1, in2, in3, in4, cntA, cntB, cntC, battery_voltage,  gyro_offset);

tel

*)

-- This file has been generated by cocoSim

#open <lustrec_math>

-- System nodes

node math_2_test (In6_1_1 : real; In7_1_1 : real; In8_1_1 : real; In9_1_1 : real; In11_1_1 : real; )

returns (Out6_1_1 : real;

	Out7_2_1 : real;

	Out8_3_1 : real;

	Out10_4_1 : real); 

var

	MathFunction5_1_1 : real;

	MathFunction6_1_1 : real;

	MathFunction7_1_1 : real;

	MathFunction9_1_1 : real;

	i_virtual_local : real;

let 

	MathFunction5_1_1 = In6_1_1 * In6_1_1;

	MathFunction6_1_1 = sqrt(In7_1_1);

	MathFunction7_1_1 = pow(In8_1_1, In9_1_1);

	MathFunction9_1_1 = 1.0 / In11_1_1;

	Out6_1_1 = MathFunction5_1_1;

	Out7_2_1 = MathFunction6_1_1;

	Out8_3_1 = MathFunction7_1_1;

	Out10_4_1 = MathFunction9_1_1;

	i_virtual_local= 0.0 -> 1.0;

tel

-- This file has been generated by cocoSim

#open <lustrec_math>

-- System nodes

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; )

returns (Out6_1_1 : real; Out6_1_2 : real; Out6_1_3 : real;

	Out7_2_1 : real; Out7_2_2 : real; Out7_2_3 : real;

	Out8_3_1 : real; Out8_3_2 : real; Out8_3_3 : real;

	Out10_4_1 : real; Out10_4_2 : real; Out10_4_3 : real;

	Out1_5_1 : real; Out1_5_2 : real; Out1_5_3 : real;

	Out2_6_1 : real; Out2_6_2 : real; Out2_6_3 : real); 

var

	MathFunction1_1_1 : real; MathFunction1_1_2 : real; MathFunction1_1_3 : real;

	MathFunction2_1_1 : real; MathFunction2_1_2 : real; MathFunction2_1_3 : real;

	MathFunction5_1_1 : real; MathFunction5_1_2 : real; MathFunction5_1_3 : real;

	MathFunction6_1_1 : real; MathFunction6_1_2 : real; MathFunction6_1_3 : real;

	MathFunction7_1_1 : real; MathFunction7_1_2 : real; MathFunction7_1_3 : real;

	MathFunction9_1_1 : real; MathFunction9_1_2 : real; MathFunction9_1_3 : real;

	i_virtual_local : real;

let 

	MathFunction1_1_1 = pow(In1_1_1, In2_1_1);

	MathFunction1_1_2 = pow(In1_1_2, In2_1_1);

	MathFunction1_1_3 = pow(In1_1_3, In2_1_1);

	MathFunction2_1_1 = pow(In3_1_1, In4_1_1);

	MathFunction2_1_2 = pow(In3_1_1, In4_1_2);

	MathFunction2_1_3 = pow(In3_1_1, In4_1_3);

	MathFunction5_1_1 = In6_1_1 * In6_1_1;

	MathFunction5_1_2 = In6_1_2 * In6_1_2;

	MathFunction5_1_3 = In6_1_3 * In6_1_3;

	MathFunction6_1_1 = sqrt(In7_1_1);

	MathFunction6_1_2 = sqrt(In7_1_2);

	MathFunction6_1_3 = sqrt(In7_1_3);

	MathFunction7_1_1 = pow(In8_1_1, In9_1_1);

	MathFunction7_1_2 = pow(In8_1_2, In9_1_2);

	MathFunction7_1_3 = pow(In8_1_3, In9_1_3);

	MathFunction9_1_1 = 1.0 / In11_1_1;

	MathFunction9_1_2 = 1.0 / In11_1_2;

	MathFunction9_1_3 = 1.0 / In11_1_3;

	Out6_1_1 = MathFunction5_1_1;

	Out6_1_2 = MathFunction5_1_2;

	Out6_1_3 = MathFunction5_1_3;

	Out7_2_1 = MathFunction6_1_1;

	Out7_2_2 = MathFunction6_1_2;

	Out7_2_3 = MathFunction6_1_3;

	Out8_3_1 = MathFunction7_1_1;

	Out8_3_2 = MathFunction7_1_2;

	Out8_3_3 = MathFunction7_1_3;

	Out10_4_1 = MathFunction9_1_1;

	Out10_4_2 = MathFunction9_1_2;

	Out10_4_3 = MathFunction9_1_3;

	Out1_5_1 = MathFunction1_1_1;

	Out1_5_2 = MathFunction1_1_2;

	Out1_5_3 = MathFunction1_1_3;

	Out2_6_1 = MathFunction2_1_1;

	Out2_6_2 = MathFunction2_1_2;

	Out2_6_3 = MathFunction2_1_3;

	i_virtual_local= 0.0 -> 1.0;

tel

-- This file has been generated by CoCoSim2.

-- Compiler: Lustre compiler 2 (ToLustre.m)

-- Time: 03-Dec-2018 22:14:40

#open <lustrec_math>

(*

Original block name: trigo_mix_test_PP

*)

node  trigo_mix_test_PP(In7_1 : real;

	In15_1 : real;

	In15_2 : real;

	In1_1 : real;

	In1_2 : real;

	In2_1 : real;

	In3_1 : real;

	In3_2 : real;

	In3_3 : real;

	In3_4 : real;

	In3_5 : real;

	In3_6 : real;

	In4_1 : real;

	In5_1 : real;

	In6_1 : real;

	In6_2 : real;

	In6_3 : real;

	In6_4 : real;

	In6_5 : real;

	In6_6 : real;)

returns(Out7_1 : real;

	Out7_2 : real;

	Out1_1 : real;

	Out1_2 : real;

	Out2_1 : real;

	Out2_2 : real;

	Out2_3 : real;

	Out2_4 : real;

	Out2_5 : real;

	Out2_6 : real;

	Out3_1 : real;

	Out3_2 : real;

	Out3_3 : real;

	Out3_4 : real;

	Out3_5 : real;

	Out3_6 : real;);

var TrigonometricFunction1_1 : real;

	TrigonometricFunction1_2 : real;

	TrigonometricFunction2_1 : real;

	TrigonometricFunction2_2 : real;

	TrigonometricFunction2_3 : real;

	TrigonometricFunction2_4 : real;

	TrigonometricFunction2_5 : real;

	TrigonometricFunction2_6 : real;

	TrigonometricFunction3_1 : real;

	TrigonometricFunction3_2 : real;

	TrigonometricFunction3_3 : real;

	TrigonometricFunction3_4 : real;

	TrigonometricFunction3_5 : real;

	TrigonometricFunction3_6 : real;

	TrigonometricFunction6_1 : real;

	TrigonometricFunction6_2 : real;

	__time_step : real;

	__nb_step : int;

let

	TrigonometricFunction1_1 = atan2(In1_1, In2_1);

	TrigonometricFunction1_2 = atan2(In1_2, In2_1);

	TrigonometricFunction2_1 = atan2(In3_1, In4_1);

	TrigonometricFunction2_2 = atan2(In3_2, In4_1);

	TrigonometricFunction2_3 = atan2(In3_3, In4_1);

	TrigonometricFunction2_4 = atan2(In3_4, In4_1);

	TrigonometricFunction2_5 = atan2(In3_5, In4_1);

	TrigonometricFunction2_6 = atan2(In3_6, In4_1);

	TrigonometricFunction3_1 = atan2(In5_1, In6_1);

	TrigonometricFunction3_2 = atan2(In5_1, In6_2);

	TrigonometricFunction3_3 = atan2(In5_1, In6_3);

	TrigonometricFunction3_4 = atan2(In5_1, In6_4);

	TrigonometricFunction3_5 = atan2(In5_1, In6_5);

	TrigonometricFunction3_6 = atan2(In5_1, In6_6);

	TrigonometricFunction6_1 = atan2(In7_1, In15_1);

	TrigonometricFunction6_2 = atan2(In7_1, In15_2);

	Out7_1 = TrigonometricFunction6_1;

	Out7_2 = TrigonometricFunction6_2;

	Out1_1 = TrigonometricFunction1_1;

	Out1_2 = TrigonometricFunction1_2;

	Out2_1 = TrigonometricFunction2_1;

	Out2_2 = TrigonometricFunction2_2;

	Out2_3 = TrigonometricFunction2_3;

	Out2_4 = TrigonometricFunction2_4;

	Out2_5 = TrigonometricFunction2_5;

	Out2_6 = TrigonometricFunction2_6;

	Out3_1 = TrigonometricFunction3_1;

	Out3_2 = TrigonometricFunction3_2;

	Out3_3 = TrigonometricFunction3_3;

	Out3_4 = TrigonometricFunction3_4;

	Out3_5 = TrigonometricFunction3_5;

	Out3_6 = TrigonometricFunction3_6;

	__time_step = (0.0 -> ((pre __time_step) + 0.200000000000000));

	__nb_step = (0 -> ((pre __nb_step) + 1));

tel