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_gainbattery_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.,fwdpwm_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_reftheta)*k_i + 0. > pre(anti_windup*0.1));

94


95

err = errtheta + 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);

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(cnt1)/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

*)
