-
Notifications
You must be signed in to change notification settings - Fork 1
/
odom_control.aesl
137 lines (114 loc) · 3.43 KB
/
odom_control.aesl
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
<!DOCTYPE aesl-source>
<network>
<!--list of global events-->
<!--list of constants-->
<constant value="10750" name="TIME_2PI_RIGHT"/>
<constant value="11300" name="TIME_2PI_LEFT"/>
<constant value="0" name="STOP"/>
<constant value="13960" name="TIME_42_CM"/>
<constant value="32500" name="RESET_VALUE"/>
<constant value="80" name="SPEED"/>
<!--show keywords state-->
<keywords flag="true"/>
<!--node thymio-II-->
<node nodeId="4378" name="thymio-II">var i
var tmp
# odometry
timer.period = [0,0]
event.args[0] = 0 # wheel left
event.args[1] = 0 # wheel right
event.args[2] = 0 # wheel left multiplicator
event.args[3] = 0 # wheel right multiplicator
event.args[10] = 0 # rotation angle
event.args[11] = 0 # distance
event.args[12] = 0 # 1 = Accept angle command, 2 = accept line command, 3 = Angle and then line,
# 4 = line and then angle, 0 = don't accept precise command
# disable leds
call leds.prox.v(0,0)
call leds.temperature(0,0)
onevent motor # odometry
if i%2 == 0 then # avoid overflow with multiplicators and increment
if event.args[0] > RESET_VALUE then
event.args[0] = event.args[0] - RESET_VALUE
event.args[2] = event.args[2] + 1
elseif event.args[0] < -RESET_VALUE then
event.args[0] = event.args[0] + RESET_VALUE
event.args[2] = event.args[2] - 1
end
if event.args[1] > RESET_VALUE then
event.args[1] = event.args[1] - RESET_VALUE
event.args[3] = event.args[3] + 1
elseif event.args[1] < -RESET_VALUE then
event.args[1] = event.args[1] + RESET_VALUE
event.args[3] = event.args[3] - 1
end
event.args[0] = event.args[0] + motor.left.speed
event.args[1] = event.args[1] + motor.right.speed
end
i = i+1
if event.args[12] == 1 or event.args[12] == 3 then
callsub turn_angle
elseif event.args[12] == 2 or event.args[12] == 4 then
callsub go_straight_distance
end
sub turn_angle
if event.args[10] > 0 then
motor.left.target = -SPEED
motor.right.target = SPEED
call math.muldiv(tmp, event.args[10], TIME_2PI_LEFT, 32767)
timer.period[0] = tmp
elseif event.args[10] < 0 then
motor.left.target = SPEED
motor.right.target = -SPEED
call math.muldiv(tmp, -event.args[10], TIME_2PI_RIGHT, 32767)
timer.period[0] = tmp
end
sub go_straight_distance
if event.args[11] > 0 then
motor.left.target = SPEED
motor.right.target = SPEED
call math.muldiv(tmp, event.args[11], TIME_42_CM, 32767)
timer.period[0] = tmp
elseif event.args[11] < 0 then
motor.left.target = -SPEED
motor.right.target = -SPEED
call math.muldiv(tmp, -event.args[11], TIME_42_CM, 32767)
timer.period[0] = tmp
end
onevent timer0
if event.args[12] == 3 then
event.args[12] = 2
callsub go_straight_distance
elseif event.args[12] == 4 then
event.args[12] = 1
callsub turn_angle
else
motor.left.target = 0
motor.right.target = 0
timer.period[0] = 0
event.args[12] = 0
end
#onevent timer1
# motor.left.target=0
# motor.right.target=0
# timer.period[1] = 0
onevent button.forward
motor.left.target = SPEED
motor.right.target = SPEED
onevent button.right
motor.left.target = SPEED
motor.right.target = -SPEED
#call math.muldiv(tmp, event.args[10], TIME_2PI_RIGHT, 32767)
#timer.period[0] = tmp
onevent button.left
motor.left.target = -SPEED
motor.right.target = SPEED
#call math.muldiv(tmp, event.args[10], TIME_2PI_LEFT, 32767)
#timer.period[0] = tmp
onevent button.backward
motor.left.target = 0
motor.right.target = 0
timer.period[0] = 0
call leds.top(0,0,0)
</node>
</network>