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

86

fwd_l = cmd_max * (err_l/(battery*battery_gainbattery_offset));

87

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

88

pwm_r = sat(100.,100.,fwd_rpwm_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_refx_11) + k_f_2*(x_12_refx_12)+ k_f_3*(x_13_refx_13) +k_f_4*(x_14_refx_14)+k_f_5*(x_15_refx_15)+k_f_6*(x_16_refx_16) + k_f_7*(x_17_refx_17);

106


107

err_l = k_f_1*(x_11_refx_11) + k_f_2*(x_12_refx_12) k_f_3*(x_13_refx_13) +k_f_4*(x_14_refx_14)+k_f_5*(x_15_refx_15)k_f_6*(x_16_refx_16) + k_f_7*(x_17_refx_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(cnt1)/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

*)
