-
Notifications
You must be signed in to change notification settings - Fork 0
/
pseudocode
275 lines (223 loc) · 7.37 KB
/
pseudocode
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
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
Research Track 1 - Assignment 1
PSEUDO-CODE
1. Initialize global variables
SET a_th TO 2
SET d_th TO 0.4
SET straight_on_speed TO 75
SET kp TO 1
SET margin_error TO 1
SET max_angle_from_g TO 80
SET min_angle_from_g TO 10
SET dist_from_g TO 1
SET turn_from_g TO 90
SET refresh_rate TO 0.05
INITIALIZE print_flag TO 0
INITIALIZE instance R of class Robot
2. Functions
FUNCTION drive WITH (speed, seconds)
SET R.motors[0].m0.power EQUALS TO speed
SET R.motors[0].m1.power EQUALS TO speed
CALL time.sleep WITH seconds
SET R.motors[0].m0.power EQUALS TO 0
SET R.motors[0].m1.power EQUALS TO 0
ENDFUNCTION
FUNCTION turn WITH (speed, seconds)
SET R.motors[0].m0.power EQUALS TO speed
SET R.motors[0].m1.power EQUALS TO -speed
CALL time.sleep WITH seconds
SET R.motors[0].m0.power EQUALS TO 0
SET R.motors[0].m1.power EQUALS TO 0
ENDFUNCTION
FUNCTION find_token_silver
SET dist TO 100
FOR token IN CALL R.see
IF token.info.marker_type IS MARKER_TOKEN_SILVER AND token.dist IS LESS THAN dist THEN
SET dist EQUALS TO token.dist
ENDIF
SET rot_y EQUALS TO token.rot_y
ENDFOR
IF dist IS EQUALS TO 100
RETURN -1, -1
ELSE
RETURN dist, rot_y
ENDIF
ENDFUNCTION
FUNCTION find_token_gold
SET dist TO 100
FOR token IN CALL R.see
IF token.info.marker_type IS MARKER_TOKEN_GOLD AND token.dist IS LESS THAN dist THEN
SET dist EQUALS TO token.dist
ENDIF
SET rot_y EQUALS TO token.rot_y
ENDFOR
IF dist IS EQUALS TO 100
RETURN -1, -1
ELSE
RETURN dist, rot_y
ENDIF
ENDFUNCTION
FUNCTION turn_P WITH (grades)
SET heading TO R.heading * 180 / math.pi
IF heading IS BETWEEN -180 and 0 THEN
SET heading TO CALL module WITH (heading)
ELIF heading IS BETWEEN 0 and 180 THEN
SET heading to 360 - heading
ENDIF
SET final TO heading + CALL module WITH (grades)
IF final IS GREATER THAN 360 THEN
SET final TO final - 360
SET error TO 360 - heading + final
SET final_flag TO 1
ELSE
SET error TO final - heading
SET error TO CALL module WITH (error)
ENDIF
WHILE error IS GREATER THAN margin_error
IF grades IS GREATER THAN 0 THEN
CALL turn WITH (-error * kp, refresh_rate)
ELSE
CALL turn WITH (+error * kp, refresh_rate)
ENDIF
SET heading TO R.heading * 180 / math.pi
IF heading IS BETWEEN -180 and 0 THEN
SET heading TO CALL module WITH (heading)
ELIF heading IS BETWEEN 0 and 180 THEN
SET heading to 360 - heading
ENDIF
IF final_flag IS EQUALS TO 1 and heading IS GREATER THAN final THEN
SET error TO 360 - heading + final
ELSE
SET error TO final - heading
SET error TO CALL module WITH (error)
ENDIF
IF error IS GREATER THAN 180 THEN
SET error TO 360 - error
ENDIF
ENDFUNCTION
FUNCTION move_behind
SET dist_1 TO 100
FOR token IN CALL R.see
IF token.info.marker_type IS MARKER_TOKEN_GOLD AND token.dist IS LESS THAN dist_1
AND token.rot_y IS BETWEEN 75 AND 105 THEN
SET dist_1 EQUALS TO token.dist
ENDIF
ENDFOR
SET dist_2 TO 100
FOR token IN CALL R.see
IF token.info.marker_type IS MARKER_TOKEN_GOLD AND token.dist IS LESS THAN dist_2
AND token.rot_y IS BETWEEN -105 AND -75 THEN
SET dist_2 EQUALS TO token.dist
ENDIF
ENDFOR
IF dist_1 IS GREATER THAN dist_2 THEN
CALL turn_P WITH (-180)
ELSE
CALL turn_P WITH (+180)
ENDIF
CALL R.release
CALL turn_P WITH (180)
ENDFUNCTION
FUNCTION module WITH (rot)
IF rot IS LESS THAN 0 THEN
RETURN rot * -1
ELSE
RETURN rot
ENDIF
ENDFUNCTION
3. Program
PRINT "Turning on the robot"
CALL time.sleep WITH (3)
PRINT "GO!"
WHILE 1
SET dist_s, rot_s CALL find_token_silver
SET dist_g, rot_g CALL find_token_gold
SET rot_s_m CALL module WITH (rot_s)
SET rot_g_m CALL module WITH (rot_g)
IF dist_s IS EQUALS TO -1 THEN
Print "I don't see any silver token... :("
ELSE
IF dist_s IS LESS THAN d_th AND rot_s_m IS LESS THAN a_th THEN
CALL R.grab
PRINT "Gotcha!"
CALL move_behind
SET print_flag TO 1
ENDIF
ENDIF
IF dist_s IS LESS THAN dist_g AND rot_s_m IS LESS THAN 90 THEN
IF print_flag IS EQUALS TO 0
PRINT "Silver token found"
SET print_flag TO 2
ELIF print_flag IS EQUALS TO 1
SET print_flag TO 0
ENDIF
IF rot_s_m IS LESS THAN a_th THEN
CALL drive WITH (+dist_s * straight_on_speed, refresh_rate)
ELSE
IF rot_s IS LESS THAN 0 THEN
CALL turn WITH (-rot_s_m, refresh_rate)
ELSE
CALL turn WITH (+rot_s_m, refresh_rate)
ENDIF
CONTINUE
ENDIF
ENDIF
IF rot_g_m IS BETWEEN min_angle_from_g and max_angle_from_g THEN
SET error TO 90 - rot_g_m
IF rot_g IS LESS THAN 0 THEN
CALL turn WITH (+error, refresh_rate)
ELSE
CALL turn WITH (-error, refresh_rate)
ENDIF
CONTINUE
ENDIF
SET dist_0 TO 100
FOR token IN CALL R.see
IF token.info.marker_type IS MARKER_TOKEN_GOLD AND token.dist IS LESS THAN dist_0
AND token.rot_y IS BETWEEN -min_angle_from_g AND min_angle_from_g THEN
SET dist_0 EQUALS TO token.dist
ENDIF
ENDFOR
IF dist_0 IS LESS THAN 1 THEN
SET dist_1 TO 100
FOR token IN CALL R.see
IF token.info.marker_type IS MARKER_TOKEN_GOLD AND token.dist IS LESS THAN dist_1
AND token.rot_y IS BETWEEN 80 AND 100 THEN
SET dist_1 EQUALS TO token.dist
ENDIF
ENDFOR
IF dist_1 IS EQUALS TO 100 THEN
PRINT "Error! I did not find any golden token on the right..."
ENDIF
SET dist_2 TO 100
FOR token IN CALL R.see
IF token.info.marker_type IS MARKER_TOKEN_GOLD AND token.dist IS LESS THAN dist_2
AND token.rot_y IS BETWEEN -100 AND -80 THEN
SET dist_2 EQUALS TO token.dist
ENDIF
ENDFOR
IF dist_2 IS EQUALS TO 100 THEN
PRINT "Error! I did not find any golden token on the left..."
ENDIF
IF dist_1 IS GREATER THAN dist_2 THEN
PRINT "Turn right"
ELSE
PRINT "Turn left"
ENDIF
WHILE dist_0 IS LESS THAN 3
IF dist_1 IS GREATER THAN dist_2 THEN
CALL turn WITH (+straight_on_speed / dist_0, refresh_rate)
ELSE
CALL turn WITH (-straight_on_speed / dist_0, refresh_rate)
ENDIF
SET rot_g CALL find_token_gold
SET rot_g_m CALL module WITH rot_g
FOR token IN CALL R.see
IF token.info.marker_type IS MARKER_TOKEN_GOLD AND token.dist IS LESS THAN dist_0
AND token.rot_y IS BETWEEN -min_angle_from_g AND min_angle_from_g THEN
SET dist_0 EQUALS TO token.dist
ENDIF
ENDFOR
ENDWHILE
ENDIF
CALL drive WITH (+straight_on_speed, refresh_rate)
ENDWHILE