Revision 1969
Traffic Navigation:
Started integration of the new intersection queue code.
(main-intersectionDebug.c, main-new.c, & traffic_navigation.h).
The new code will not be compiled by default.
Additionally, I added #defines to all orb control statements because
they were interfering with each other. To turn some orb control statements on,
define them in traffic_navigation.h
trunk/code/projects/traffic_navigation/main-intersectionDebug.c | ||
---|---|---|
1 |
#ifdef INTERSECTION_MAIN_DEBUG |
|
2 |
/* |
|
3 |
* main.c for Traffic Navigation, Intersection queue debugging (version 2) |
|
4 |
* Runs the highest level behavior for the Dynamic Traffic Navigation (DTM) SURG |
|
5 |
* |
|
6 |
* Author: Colony Project, CMU Robotics Club |
|
7 |
*/ |
|
8 |
#include "traffic_navigation.h" |
|
9 |
|
|
10 |
static int state, sign, turnDir; |
|
11 |
//resolvPrevBotID: -3 : not initialized |
|
12 |
// -2 : recieved a resolv packet but still have not |
|
13 |
// found someone to be before me |
|
14 |
// -1 : bot that was there is done getting in queue |
|
15 |
// otherwise, that bot needs to get in the queue before this one |
|
16 |
static char sendBuffer[PACKET_LENGTH], queuePrevBot, queueNextBot, id, intersectionNum, resolvPrevBotID = -3; |
|
17 |
unsigned char resolvSeed = 0xC9, resolvDraw = 0, resolvPrevBotDraw = 0; |
|
18 |
|
|
19 |
/* Wireless packet parsing */ |
|
20 |
int wirelessPacketHandle(int state); |
|
21 |
void enterIntersection(void); |
|
22 |
void sendResolv(bool override); |
|
23 |
unsigned char resolvRandomNumberGen(); |
|
24 |
|
|
25 |
|
|
26 |
int main (void) { |
|
27 |
|
|
28 |
/* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */ |
|
29 |
dragonfly_init(ALL_ON); |
|
30 |
xbee_init(); |
|
31 |
encoders_init(); |
|
32 |
rtc_init(SIXTEENTH_SECOND, NULL); |
|
33 |
wl_basic_init_default(); |
|
34 |
wl_set_channel(13); |
|
35 |
initializeData(); |
|
36 |
|
|
37 |
id = get_robotid(); |
|
38 |
sign = 0; |
|
39 |
|
|
40 |
//Test code |
|
41 |
state = SROAD; |
|
42 |
|
|
43 |
sendBuffer[1] = id; |
|
44 |
|
|
45 |
/* |
|
46 |
doDrive(200); |
|
47 |
turn(DOUBLE, ILEFT); |
|
48 |
*/ |
|
49 |
|
|
50 |
while (1) { |
|
51 |
/*DTM Finite State Machine*/ |
|
52 |
switch(state){ |
|
53 |
case SROAD:/*Following a normal road*/ |
|
54 |
|
|
55 |
//idle parsing |
|
56 |
wirelessPacketHandle(SROAD); |
|
57 |
|
|
58 |
// sign = lineFollow(); |
|
59 |
//other road behaviors |
|
60 |
//tailgating? |
|
61 |
//read barcode |
|
62 |
orb1_set_color(ORB_OFF); |
|
63 |
orb2_set_color(ORB_OFF); |
|
64 |
if(button1_click()){ |
|
65 |
state = SINTERSECTION_ENTER; |
|
66 |
} |
|
67 |
break; |
|
68 |
case SINTERSECTION_ENTER:/*Entering, and in intersection*/ |
|
69 |
orb1_set_color(RED); |
|
70 |
orb2_set_color(ORB_OFF); |
|
71 |
|
|
72 |
usb_puts("STATE: SINTERSECTION_ENTER\n"); |
|
73 |
|
|
74 |
//empty packet queue |
|
75 |
//while(wirelessPacketHandle(SCLEARPACKET) != ENOPACKET); |
|
76 |
|
|
77 |
/*Intersection queue: |
|
78 |
*Each robot when entering the intersection will check for other robots |
|
79 |
*in the intersection, and insert itself in a queue to go through. |
|
80 |
*/ |
|
81 |
queuePrevBot = -1; |
|
82 |
queueNextBot = -1; |
|
83 |
resolvPrevBotID = -3; |
|
84 |
resolvPrevBotDraw = 0; |
|
85 |
resolvDraw = 0; |
|
86 |
|
|
87 |
sign = 0; //Test code until barcodes integrated |
|
88 |
intersectionNum = 0; //actually set this. |
|
89 |
turnDir = 1; |
|
90 |
|
|
91 |
enterIntersection(); //sends wireless packet for entry |
|
92 |
state = SINTERSECTION_WAIT; |
|
93 |
|
|
94 |
rtc_reset(); //reset rtc for timeout wait for reply |
|
95 |
|
|
96 |
bool done = false; |
|
97 |
bool retried = false; |
|
98 |
while(rtc_get() < 16 && !done){//waits for a reply, otherwise assumes it is first in queue |
|
99 |
int ret = wirelessPacketHandle(SINTERSECTION_ENTER); |
|
100 |
if(rtc_get() > 12 && !retried){//by now all resolvs should be done from bots that arrived earlier... |
|
101 |
orb2_set_color(PURPLE); |
|
102 |
enterIntersection(); |
|
103 |
retried = true; |
|
104 |
} |
|
105 |
switch (ret) { |
|
106 |
case KPLACEDINQUEUE: |
|
107 |
orb2_set_color(GREEN); //entered queue! |
|
108 |
done = true; |
|
109 |
break; |
|
110 |
case KFAILEDTOQUEUE: |
|
111 |
usb_puts("Failed to queue D:\n"); |
|
112 |
orb2_set_color(RED); |
|
113 |
enterIntersection(); |
|
114 |
rtc_reset(); |
|
115 |
break; |
|
116 |
case KRESOLVINGENTER: |
|
117 |
orb2_set_color(ORANGE); |
|
118 |
state = SINTERSECTION_ENTER_RESOLV; |
|
119 |
done = true; |
|
120 |
break; |
|
121 |
} |
|
122 |
} |
|
123 |
break; |
|
124 |
case SINTERSECTION_ENTER_RESOLV: |
|
125 |
orb1_set_color(PINK); |
|
126 |
orb2_set_color(ORB_OFF); |
|
127 |
|
|
128 |
usb_puts("STATE: SINTERSECTION_ENTER_RESOLV\n"); |
|
129 |
rtc_reset(); |
|
130 |
|
|
131 |
done = false; |
|
132 |
retried = false; |
|
133 |
while(rtc_get() < 9 && !done){ |
|
134 |
int ret = wirelessPacketHandle(SINTERSECTION_ENTER_RESOLV); |
|
135 |
switch (ret) { |
|
136 |
case KRESOLVINGENTER: |
|
137 |
orb2_set_color(YELLOW); |
|
138 |
break; |
|
139 |
case KPLACEDINQUEUE: |
|
140 |
orb2_set_color(GREEN); //entered queue! |
|
141 |
done = true; |
|
142 |
break; |
|
143 |
case KFAILEDTOQUEUE: |
|
144 |
usb_puts("Failed to queue D:\n"); |
|
145 |
orb2_set_color(RED); |
|
146 |
enterIntersection(); |
|
147 |
rtc_reset(); |
|
148 |
break; |
|
149 |
} |
|
150 |
if(!done && resolvPrevBotID == (char) -1 && !retried){ |
|
151 |
enterIntersection(); |
|
152 |
rtc_reset(); |
|
153 |
retried = true; |
|
154 |
} else if(!done && resolvPrevBotID == (char) -2 && rtc_get() > 6){ |
|
155 |
//send a intersection reply to myself to |
|
156 |
//trigger other bots to enter queue. |
|
157 |
sendBuffer[0] = WINTERSECTIONREPLY; |
|
158 |
sendBuffer[2] = intersectionNum; |
|
159 |
sendBuffer[3] = id; |
|
160 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
161 |
|
|
162 |
done = true; |
|
163 |
break; |
|
164 |
} |
|
165 |
} |
|
166 |
state = SINTERSECTION_WAIT; |
|
167 |
break; |
|
168 |
case SINTERSECTION_WAIT:/*Waiting in intersection */ |
|
169 |
orb1_set_color(YELLOW); |
|
170 |
orb2_set_color(ORB_OFF); |
|
171 |
usb_puts("STATE: SINTERSECTION_WAIT\n"); |
|
172 |
|
|
173 |
while(queuePrevBot != (char) -1){ |
|
174 |
int ret = wirelessPacketHandle(state); |
|
175 |
switch (ret){ |
|
176 |
case KFIRSTINQUEUE: |
|
177 |
orb2_set_color(GREEN); |
|
178 |
state = SINTERSECTION_DRIVE; |
|
179 |
break; |
|
180 |
case KREPLIEDTOENTER: |
|
181 |
orb2_set_color(BLUE); |
|
182 |
break; |
|
183 |
case KRESOLVINGENTER: |
|
184 |
orb2_set_color(ORANGE); |
|
185 |
break; |
|
186 |
} |
|
187 |
} |
|
188 |
state = SINTERSECTION_DRIVE; |
|
189 |
break; |
|
190 |
case SINTERSECTION_DRIVE: |
|
191 |
usb_puts("STATE: SINTERSECTION_DRIVE\n"); |
|
192 |
orb1_set_color(GREEN); |
|
193 |
orb2_set_color(ORB_OFF); |
|
194 |
|
|
195 |
while(!button2_click()){ |
|
196 |
int ret = wirelessPacketHandle(state); |
|
197 |
switch (ret){ |
|
198 |
case KREPLIEDTOENTER: |
|
199 |
orb2_set_color(BLUE); |
|
200 |
break; |
|
201 |
case KRESOLVINGENTER: |
|
202 |
orb2_set_color(ORANGE); |
|
203 |
break; |
|
204 |
} |
|
205 |
} |
|
206 |
|
|
207 |
//Exits intersection |
|
208 |
orb1_set_color(WHITE); |
|
209 |
|
|
210 |
sendBuffer[0] = WINTERSECTIONEXIT; |
|
211 |
sendBuffer[2] = intersectionNum;//Intersection # |
|
212 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
213 |
|
|
214 |
state = SROAD; |
|
215 |
break; |
|
216 |
default: |
|
217 |
usb_puts("I got stuck in an unknown state! My state is "); |
|
218 |
usb_puti(state); |
|
219 |
} |
|
220 |
} |
|
221 |
|
|
222 |
} |
|
223 |
|
|
224 |
int wirelessPacketHandle(int state){ |
|
225 |
int dataLength = 0; |
|
226 |
unsigned char *packet = NULL; |
|
227 |
packet = wl_basic_do_default(&dataLength); |
|
228 |
|
|
229 |
/* sanity check */ |
|
230 |
if(dataLength == 0 || packet == NULL) //no packet |
|
231 |
return ENOPACKET; |
|
232 |
|
|
233 |
if(dataLength != PACKET_LENGTH) |
|
234 |
return EPACKETLEN; |
|
235 |
|
|
236 |
usb_puts("Recieved Wireless Packet: "); |
|
237 |
for(int i = 0; i < dataLength; i++){ |
|
238 |
usb_puti(packet[i]); |
|
239 |
usb_putc(' '); |
|
240 |
} |
|
241 |
usb_putc('\n'); |
|
242 |
|
|
243 |
switch (packet[0]){ |
|
244 |
case WROADENTRY: //[type, bot, road] |
|
245 |
return ENOACTION; |
|
246 |
break; |
|
247 |
case WROADEXIT: //[type, bot, road] |
|
248 |
return ENOACTION; |
|
249 |
break; |
|
250 |
case WROADSTOP: //[type, bot, road] |
|
251 |
return ENOACTION; |
|
252 |
break; |
|
253 |
case WINTERSECTIONENTRY: //[type, bot, intersection, fromDir, toDir] |
|
254 |
if (packet[2] == intersectionNum){ |
|
255 |
switch (state){ |
|
256 |
case SINTERSECTION_ENTER: |
|
257 |
sendResolv(false); |
|
258 |
resolvPrevBotID = -2; |
|
259 |
return KRESOLVINGENTER; |
|
260 |
break; |
|
261 |
case SINTERSECTION_ENTER_RESOLV: |
|
262 |
return ENOACTION; |
|
263 |
case SINTERSECTION_WAIT: |
|
264 |
case SINTERSECTION_DRIVE: |
|
265 |
if(queueNextBot == (char) -1){ |
|
266 |
sendBuffer[0] = WINTERSECTIONREPLY; |
|
267 |
sendBuffer[2] = intersectionNum; |
|
268 |
sendBuffer[3] = packet[1]; |
|
269 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
270 |
queueNextBot = packet[1]; |
|
271 |
return KREPLIEDTOENTER; |
|
272 |
} |
|
273 |
break; |
|
274 |
|
|
275 |
} |
|
276 |
} |
|
277 |
break; |
|
278 |
case WINTERSECTIONREPLY: //[type, fromBot, intersection, toBot] |
|
279 |
if (packet[2] == intersectionNum){ |
|
280 |
switch (state){ |
|
281 |
case SINTERSECTION_ENTER: |
|
282 |
case SINTERSECTION_ENTER_RESOLV: |
|
283 |
if(packet[3] == id){ //Reply for me |
|
284 |
queuePrevBot = packet[1]; |
|
285 |
return KPLACEDINQUEUE; |
|
286 |
} else { |
|
287 |
if(packet[3] == resolvPrevBotID) |
|
288 |
resolvPrevBotID = -1; |
|
289 |
return KFAILEDTOQUEUE; |
|
290 |
} |
|
291 |
break; |
|
292 |
default: |
|
293 |
return ENOACTION; |
|
294 |
} |
|
295 |
} |
|
296 |
break; |
|
297 |
case WINTERSECTIONEXIT: //[type, bot, intersection] |
|
298 |
if (packet[2] == intersectionNum){ |
|
299 |
switch (state){ |
|
300 |
case SINTERSECTION_WAIT: |
|
301 |
if(packet[1]==queuePrevBot){ |
|
302 |
queuePrevBot=-1; |
|
303 |
return KFIRSTINQUEUE; |
|
304 |
} |
|
305 |
} |
|
306 |
} |
|
307 |
break; |
|
308 |
case WINTERSECTIONGO: //[type, bot, intersection] |
|
309 |
break; |
|
310 |
case WINTERSECTIONPOLICEENTRY: //[?] |
|
311 |
return ENOACTION; |
|
312 |
break; |
|
313 |
case WINTERSECTIONRESOLVERACE: //[type, bot, intersection, num] |
|
314 |
//in the case robots draw the same number these will be |
|
315 |
//arbitrated using the sending robot's id, prefering |
|
316 |
//lower ids |
|
317 |
usb_puts("Now in wireless WINTERSECTIONRESOLVERACE handler: resolvPrevBotID: "); |
|
318 |
usb_puti((int) resolvPrevBotID); |
|
319 |
usb_putc('\n'); |
|
320 |
if (packet[2] == intersectionNum){ |
|
321 |
switch (state){ |
|
322 |
case SINTERSECTION_ENTER: |
|
323 |
case SINTERSECTION_ENTER_RESOLV: |
|
324 |
if(resolvPrevBotID == (char) -3){ |
|
325 |
usb_puts("resolvPrevBotID == -3; sending a resolv packet and setting to -2\n"); |
|
326 |
sendResolv(false); |
|
327 |
resolvPrevBotID = -2; |
|
328 |
} |
|
329 |
if((unsigned char) packet[3] == resolvDraw && id > packet[1]){ |
|
330 |
//other bot drew same number as me, |
|
331 |
//and i have a higher id, so it goes first. |
|
332 |
usb_puts("bot "); |
|
333 |
usb_puti(packet[1]); |
|
334 |
usb_puts(" Drew the same number as me and I have a higher id, so it goes first\n"); |
|
335 |
resolvPrevBotID = packet[1]; |
|
336 |
} else if((unsigned char) packet[3] == resolvPrevBotDraw && resolvPrevBotID > packet[1]){ |
|
337 |
//other bot drew same number as the bot before me, |
|
338 |
//so if it has a higher id than the bot before me, |
|
339 |
//it is going to go in between that one and me. |
|
340 |
usb_puts("bot "); |
|
341 |
usb_puti(packet[1]); |
|
342 |
usb_puts(" Drew the same number as the bot before me, and the bot before me has have a higher id, so this goes first\n"); |
|
343 |
resolvPrevBotID = packet[1]; |
|
344 |
} else if((unsigned char)packet[3] < resolvDraw && (unsigned char)packet[3] > resolvPrevBotDraw){ |
|
345 |
//found a bot that goes in between the bot before me |
|
346 |
//and me, so now it is before me. |
|
347 |
usb_puts("bot "); |
|
348 |
usb_puti(packet[1]); |
|
349 |
usb_puts(" Drew a lower number bot before me, and the bot before me has have a higher id, so this goes first\n"); |
|
350 |
resolvPrevBotDraw = packet[3]; |
|
351 |
resolvPrevBotID = packet[1]; |
|
352 |
} |
|
353 |
return KRESOLVINGENTER; |
|
354 |
break; |
|
355 |
case SINTERSECTION_WAIT: |
|
356 |
case SINTERSECTION_DRIVE: |
|
357 |
usb_puts("Trying to resolv in non resolv case...queueNextbot = "); usb_puti(queueNextBot); usb_puts("\n"); |
|
358 |
if(queueNextBot == (char) -1){ |
|
359 |
sendResolv(true); |
|
360 |
} |
|
361 |
return KRESOLVINGENTER; |
|
362 |
break; |
|
363 |
} |
|
364 |
|
|
365 |
} |
|
366 |
break; |
|
367 |
case WHIGHWAYENTRY: //[type, bot, highway] |
|
368 |
return ENOACTION; |
|
369 |
break; |
|
370 |
case WHIGHWAYREPLY: //[type, fromBot, highway, toBot] |
|
371 |
return ENOACTION; |
|
372 |
break; |
|
373 |
case WHIGHWAYEXIT: //[type, bot, highway] |
|
374 |
return ENOACTION; |
|
375 |
break; |
|
376 |
case WPINGGLOBAL: //[type, bot] |
|
377 |
return ENOACTION; |
|
378 |
break; |
|
379 |
case WPINGBOT: //[type, fromBot, toBot] |
|
380 |
return ENOACTION; |
|
381 |
break; |
|
382 |
case WPINGQUEUE: //[type, fromBot, toBot] |
|
383 |
return ENOACTION; |
|
384 |
break; |
|
385 |
case WPINGREPLY: //[type, fromBot, toBot] |
|
386 |
return ENOACTION; |
|
387 |
break; |
|
388 |
case WCOLLISIONAVOID: //[type, bot, intersection, collision-int] //Note: collision is an int and thus takes two spaces |
|
389 |
return ENOACTION; |
|
390 |
break; |
|
391 |
default: |
|
392 |
return ENOACTION; |
|
393 |
break; |
|
394 |
} |
|
395 |
} |
|
396 |
|
|
397 |
unsigned char resolvRandomNumberGen(){ |
|
398 |
if ((resolvSeed *= (rtc_get())%9) == 0){ |
|
399 |
return resolvSeed + 1; //0 is a reseved priority value for the last |
|
400 |
//bot that is already in the queue. |
|
401 |
} |
|
402 |
return resolvSeed; |
|
403 |
} |
|
404 |
void sendResolv(bool override){ |
|
405 |
if(!override) |
|
406 |
resolvDraw = resolvRandomNumberGen(); |
|
407 |
else |
|
408 |
resolvDraw = 0; |
|
409 |
sendBuffer[0] = WINTERSECTIONRESOLVERACE; |
|
410 |
sendBuffer[2] = intersectionNum; |
|
411 |
sendBuffer[3] = resolvDraw; |
|
412 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
413 |
} |
|
414 |
void enterIntersection(void){ |
|
415 |
//Sends packet announcing its entry to the intersection |
|
416 |
sendBuffer[0] = WINTERSECTIONENTRY; |
|
417 |
sendBuffer[2] = intersectionNum;//Intersection # |
|
418 |
sendBuffer[3] = 0; //getIntersectPos(sign); |
|
419 |
sendBuffer[4] = turnDir; |
|
420 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
421 |
} |
|
422 |
#endif |
trunk/code/projects/traffic_navigation/traffic_navigation.h | ||
---|---|---|
7 | 7 |
#include "intersectData.h" |
8 | 8 |
#include "validTurns.h" |
9 | 9 |
|
10 |
/* Debug Options */ |
|
11 |
#ifdef DEBUGALL |
|
12 |
#define ORB_HIGHWAY |
|
13 |
#define ORB_LINEFOLLOW |
|
14 |
#define ORB_INTERSECTON |
|
15 |
#define DEBUG_CA |
|
16 |
#define DEBUG_INTERSECTION |
|
17 |
#endif |
|
18 |
|
|
10 | 19 |
/*States*/ |
11 | 20 |
#define SROAD 0 |
12 |
#define SINTERSECTION 10 |
|
21 |
#define SINTERSECTION 9 /* for old version */ |
|
22 |
#define SINTERSECTION_ENTER 10 |
|
23 |
#define SINTERSECTION_ENTER_RESOLV 11 |
|
24 |
#define SINTERSECTION_WAIT 12 |
|
25 |
#define SINTERSECTION_DRIVE 13 |
|
13 | 26 |
#define SHIGHWAY 20 |
27 |
#define SCLEARPACKET 30 |
|
14 | 28 |
|
15 | 29 |
/*Sign Codes |
16 | 30 |
* bitwise OR labels to create a barcode or read one |
... | ... | |
42 | 56 |
#define WROADREPLY 1 //[type, fromBot, road, toBot] |
43 | 57 |
#define WROADEXIT 2 //[type, bot, road] |
44 | 58 |
#define WROADSTOP 3 //[type, bot, road] |
45 |
#define WINTERSECTIONENTRY 10 //[type, bot, intersection, fromDir, toDir] |
|
46 |
#define WINTERSECTIONREPLY 11 //[type, fromBot, intersection, toBot] |
|
47 |
#define WINTERSECTIONEXIT 12 //[type, bot, intersection] |
|
48 |
#define WINTERSECTIONGO 13 //[type, bot, intersection] |
|
59 |
#define WINTERSECTIONENTRY 10 //[type, bot, intersection, fromDir, toDir]
|
|
60 |
#define WINTERSECTIONREPLY 11 //[type, fromBot, intersection, toBot]
|
|
61 |
#define WINTERSECTIONEXIT 12 //[type, bot, intersection]
|
|
62 |
#define WINTERSECTIONGO 13 //[type, bot, intersection]
|
|
49 | 63 |
#define WINTERSECTIONPOLICEENTRY 14 |
64 |
#define WINTERSECTIONRESOLVERACE 15 //[type, bot, intersection, num] |
|
50 | 65 |
#define WHIGHWAYENTRY 20 //[type, bot, highway] |
51 | 66 |
#define WHIGHWAYREPLY 21 //[type, fromBot, highway, toBot] |
52 | 67 |
#define WHIGHWAYEXIT 22 //[type, bot, highway] |
... | ... | |
56 | 71 |
#define WPINGREPLY 33 //[type, fromBot, toBot] |
57 | 72 |
#define WCOLLISIONAVOID 41 //[type, bot, intersection, collision-int] //Note: collision is an int and thus takes two spaces |
58 | 73 |
|
74 |
/*Wireless Parsing Status |
|
75 |
* For wireless parsing - status codes describing various errors/statuses with wireless |
|
76 |
* parsing. |
|
77 |
* Errors begin with E - Note that all of these errors are actually returned as negative. |
|
78 |
* - values of >= 100 indicate a serious error |
|
79 |
* Non-Errors begin with K - returned as positive. (don't start with S because S |
|
80 |
* is for state) |
|
81 |
*/ |
|
82 |
#define ENOPACKET -10 //The received packet doesn't exist. |
|
83 |
#define ENOACTION -15 //The received packet has no defined action, and this is OK |
|
84 |
#define ENOIMPLEMENT -20 //The received packet has no defined action and needs to be implemented. |
|
85 |
#define EPACKETLEN -100 //The received packet was the wrong length |
|
86 |
#define KOK 1 |
|
87 |
#define KPLACEDINQUEUE 20 |
|
88 |
#define KFAILEDTOQUEUE 21 |
|
89 |
#define KFIRSTINQUEUE 22 |
|
90 |
#define KREPLIEDTOENTER 23 |
|
91 |
#define KRESOLVINGENTER 24 //resolving a race between bots trying to enter intersection at the same time. |
|
92 |
|
|
59 | 93 |
/*Macros |
60 | 94 |
*/ |
61 | 95 |
#define ISPING(p) ((p)[0]==WPINGGLOBAL || (p)[0]==WPINGBOT || (p)[0]==WPINGQUEUE) |
trunk/code/projects/traffic_navigation/main-new.c | ||
---|---|---|
1 |
#ifdef MAIN_NEW |
|
2 |
/* |
|
3 |
* main.c for Traffic Navigation |
|
4 |
* Runs the highest level behavior for the Dynamic Traffic Navigation (DTM) SURG |
|
5 |
* |
|
6 |
* Author: Colony Project, CMU Robotics Club |
|
7 |
*/ |
|
8 |
|
|
9 |
#include "traffic_navigation.h" |
|
10 |
#include "../linefollowing/lineDrive.h" |
|
11 |
|
|
12 |
static int state, sign, turnDir; |
|
13 |
static char sendBuffer[PACKET_LENGTH], queuePrevBot, queueNextBot, id, nextDir, nextPath, intersectionNum, resolvPrevBotID = -3; |
|
14 |
unsigned char resolvSeed = 0xC9, resolvDraw = 0, resolvPrevBotDraw = 0; |
|
15 |
|
|
16 |
int wirelessPacketHandle(int state); |
|
17 |
void enterIntersection(void); |
|
18 |
void sendResolv(bool override); |
|
19 |
unsigned char resolvRandomNumberGen(); |
|
20 |
|
|
21 |
int main (void) { |
|
22 |
|
|
23 |
/* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */ |
|
24 |
dragonfly_init(ALL_ON); |
|
25 |
xbee_init(); |
|
26 |
encoders_init(); |
|
27 |
lineDrive_init(); |
|
28 |
rtc_init(SIXTEENTH_SECOND, NULL); |
|
29 |
wl_basic_init_default(); |
|
30 |
wl_set_channel(13); |
|
31 |
initializeData(); |
|
32 |
|
|
33 |
id = get_robotid(); |
|
34 |
sign = 0; |
|
35 |
|
|
36 |
//Test code |
|
37 |
state = SROAD; |
|
38 |
|
|
39 |
sendBuffer[1] = id; |
|
40 |
|
|
41 |
/* |
|
42 |
doDrive(200); |
|
43 |
turn(DOUBLE, ILEFT); |
|
44 |
*/ |
|
45 |
/* |
|
46 |
while(1) |
|
47 |
doDrive(255); |
|
48 |
*/ |
|
49 |
|
|
50 |
while (1) { |
|
51 |
/*DTM Finite State Machine*/ |
|
52 |
switch(state){ |
|
53 |
case SROAD:/*Following a normal road*/ |
|
54 |
/* implement other road behaviors? |
|
55 |
* -tailgating |
|
56 |
*/ |
|
57 |
//read barcode |
|
58 |
sign = doDrive(200); |
|
59 |
#ifdef ORB_INTERSECTION |
|
60 |
orb1_set_color(ORB_OFF); |
|
61 |
orb2_set_color(ORB_OFF); |
|
62 |
#endif |
|
63 |
if(button1_click()){ |
|
64 |
state = SINTERSECTION_ENTER; |
|
65 |
} |
|
66 |
break; |
|
67 |
case SINTERSECTION_ENTER:/*Entering, and in intersection*/ |
|
68 |
stop(); |
|
69 |
doDrive(0); |
|
70 |
#ifdef ORB_INTERSECTION |
|
71 |
orb1_set_color(RED); |
|
72 |
orb2_set_color(ORB_OFF); |
|
73 |
#endif |
|
74 |
|
|
75 |
#ifdef DEBUG_INTERSECTION |
|
76 |
usb_puts("STATE: SINTERSECTION_ENTER\n"); |
|
77 |
#endif |
|
78 |
|
|
79 |
/*Intersection queue: |
|
80 |
*Each robot when entering the intersection will check for other robots |
|
81 |
*in the intersection, and insert itself in a queue to go through. |
|
82 |
*/ |
|
83 |
queuePrevBot = -1; //the bot that will drive before this bot |
|
84 |
queueNextBot = -1; //the bot that will drive after this bot |
|
85 |
resolvPrevBotID = -3; //in the case of a race, the bot that is |
|
86 |
//to enter the queue before this bot |
|
87 |
resolvPrevBotDraw = 0; //the random priority number that bot has |
|
88 |
resolvDraw = 0; //my random priority number |
|
89 |
|
|
90 |
sign = 0; //Test code until barcodes integrated |
|
91 |
intersectionNum = getIntersectType(sign); |
|
92 |
turnDir = validateTurn(sign, getTurnType(4)); |
|
93 |
|
|
94 |
enterIntersection(); //sends wireless packet for entry |
|
95 |
state = SINTERSECTION_WAIT; |
|
96 |
|
|
97 |
rtc_reset(); //reset rtc for timeout wait for reply |
|
98 |
bool done = false; |
|
99 |
char retried = 0; |
|
100 |
while(rtc_get() < 16 && !done){//waits for a reply, otherwise assumes it is first in queue |
|
101 |
int ret = wirelessPacketHandle(SINTERSECTION_ENTER); |
|
102 |
if(rtc_get() > 12 && !retried){//by now all resolvs should be done from bots that arrived earlier... |
|
103 |
#ifdef ORB_INTERSECTION |
|
104 |
orb2_set_color(PURPLE); |
|
105 |
#endif |
|
106 |
enterIntersection(); |
|
107 |
retried = 1; |
|
108 |
} |
|
109 |
switch (ret) { |
|
110 |
case KPLACEDINQUEUE: |
|
111 |
#ifdef ORB_INTERSECTION |
|
112 |
orb2_set_color(GREEN); |
|
113 |
#endif |
|
114 |
done = true; |
|
115 |
break; |
|
116 |
case KFAILEDTOQUEUE: |
|
117 |
#ifdef DEBUG_INTERSECTION |
|
118 |
usb_puts("Failed to queue\n"); |
|
119 |
#endif |
|
120 |
#ifdef ORB_INTERSECTION |
|
121 |
orb2_set_color(RED); |
|
122 |
#endif |
|
123 |
enterIntersection(); |
|
124 |
rtc_reset(); |
|
125 |
break; |
|
126 |
case KRESOLVINGENTER: |
|
127 |
|
|
128 |
#ifdef ORB_INTERSECTION |
|
129 |
orb2_set_color(ORANGE); |
|
130 |
#endif |
|
131 |
state = SINTERSECTION_ENTER_RESOLV; |
|
132 |
done = true; |
|
133 |
break; |
|
134 |
} |
|
135 |
} |
|
136 |
break; |
|
137 |
|
|
138 |
case SINTERSECTION_ENTER_RESOLV: |
|
139 |
#ifdef ORB_INTERSECTION |
|
140 |
orb1_set_color(WHITE); |
|
141 |
orb2_set_color(ORB_OFF); |
|
142 |
#endif |
|
143 |
#ifdef DEBUG_INTERSECTION |
|
144 |
usb_puts("STATE: SINTERSECTION_ENTER_RESOLV\n"); |
|
145 |
#endif |
|
146 |
|
|
147 |
rtc_reset(); |
|
148 |
done = false; |
|
149 |
retried = 0; |
|
150 |
while(rtc_get() < 9 && !done){ |
|
151 |
int ret = wirelessPacketHandle(SINTERSECTION_ENTER_RESOLV); |
|
152 |
switch (ret) { |
|
153 |
case KRESOLVINGENTER: |
|
154 |
#ifdef ORB_INTERSECTION |
|
155 |
orb2_set_color(YELLOW); |
|
156 |
#endif |
|
157 |
break; |
|
158 |
case KPLACEDINQUEUE: |
|
159 |
#ifdef ORB_INTERSECTION |
|
160 |
orb2_set_color(GREEN); |
|
161 |
#endif |
|
162 |
done = true; |
|
163 |
break; |
|
164 |
case KFAILEDTOQUEUE: |
|
165 |
usb_puts("Failed to queue\n"); |
|
166 |
orb2_set_color(RED); |
|
167 |
enterIntersection(); |
|
168 |
rtc_reset(); |
|
169 |
break; |
|
170 |
} |
|
171 |
//if resolvPrevBotID == -1, this indicates that |
|
172 |
//there was a prevbot before, but it has entered |
|
173 |
//the queue so it's our turn. |
|
174 |
if(!done && resolvPrevBotID == (char) -1 && !retried){ |
|
175 |
enterIntersection(); |
|
176 |
rtc_reset(); |
|
177 |
retried = 1; |
|
178 |
//if resolvPrevBotID == -2, we have been |
|
179 |
//resolving but never have seen a bot with lower |
|
180 |
//priority than us. after the 6/16ths sec |
|
181 |
//timeout, assume we are first. |
|
182 |
} else if(!done && resolvPrevBotID == (char) -2 && rtc_get() > 6){ |
|
183 |
//send a intersection reply to myself to |
|
184 |
//trigger other bots to enter queue. |
|
185 |
sendBuffer[0] = WINTERSECTIONREPLY; |
|
186 |
sendBuffer[2] = intersectionNum; |
|
187 |
sendBuffer[3] = id; |
|
188 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
189 |
|
|
190 |
done = true; |
|
191 |
break; |
|
192 |
} |
|
193 |
} |
|
194 |
state = SINTERSECTION_WAIT; |
|
195 |
break; |
|
196 |
case SINTERSECTION_WAIT:/*Waiting in intersection */ |
|
197 |
#ifdef ORB_INTERSECTION |
|
198 |
orb1_set_color(YELLOW); |
|
199 |
orb2_set_color(ORB_OFF); |
|
200 |
#endif |
|
201 |
#ifdef DEBUG_INTERSECTION |
|
202 |
usb_puts("STATE: SINTERSECTION_WAIT\n"); |
|
203 |
#endif |
|
204 |
|
|
205 |
while(queuePrevBot != (char) -1){ |
|
206 |
int ret = wirelessPacketHandle(state); |
|
207 |
switch (ret){ |
|
208 |
case KFIRSTINQUEUE: |
|
209 |
#ifdef ORB_INTERSECTION |
|
210 |
orb2_set_color(GREEN); |
|
211 |
#endif |
|
212 |
state = SINTERSECTION_DRIVE; |
|
213 |
break; |
|
214 |
case KREPLIEDTOENTER: |
|
215 |
#ifdef ORB_INTERSECTION |
|
216 |
orb2_set_color(BLUE); |
|
217 |
#endif |
|
218 |
break; |
|
219 |
case KRESOLVINGENTER: |
|
220 |
#ifdef ORB_INTERSECTION |
|
221 |
orb2_set_color(ORANGE); |
|
222 |
#endif |
|
223 |
break; |
|
224 |
} |
|
225 |
} |
|
226 |
state = SINTERSECTION_DRIVE; |
|
227 |
break; |
|
228 |
|
|
229 |
case SINTERSECTION_DRIVE: |
|
230 |
#ifdef DEBUG_INTERSECTION |
|
231 |
usb_puts("STATE: SINTERSECTION_DRIVE\n"); |
|
232 |
#endif |
|
233 |
#ifdef ORB_INTERSECTION |
|
234 |
orb1_set_color(GREEN); |
|
235 |
orb2_set_color(ORB_OFF); |
|
236 |
#endif |
|
237 |
start(); |
|
238 |
turn(getIntersectType(0), turnDir); |
|
239 |
while(doDrive(200) != FINISHED){ |
|
240 |
//while(!button2_click()){ |
|
241 |
int ret = wirelessPacketHandle(state); |
|
242 |
switch (ret){ |
|
243 |
case KREPLIEDTOENTER: |
|
244 |
#ifdef ORB_INTERSECTION |
|
245 |
orb2_set_color(BLUE); |
|
246 |
#endif |
|
247 |
break; |
|
248 |
case KRESOLVINGENTER: |
|
249 |
#ifdef ORB_INTERSECTION |
|
250 |
orb2_set_color(ORANGE); |
|
251 |
#endif |
|
252 |
break; |
|
253 |
} |
|
254 |
} |
|
255 |
|
|
256 |
//Exits intersection |
|
257 |
#ifdef ORB_INTERSECTION |
|
258 |
orb1_set_color(WHITE); |
|
259 |
#endif |
|
260 |
|
|
261 |
sendBuffer[0] = WINTERSECTIONEXIT; |
|
262 |
sendBuffer[2] = intersectionNum;//Intersection # |
|
263 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
264 |
|
|
265 |
//Exits intersection |
|
266 |
while(1){ |
|
267 |
if(button1_click()){ |
|
268 |
start(); |
|
269 |
state = SHIGHWAY; |
|
270 |
break; |
|
271 |
} |
|
272 |
if(button2_click()){ |
|
273 |
start(); |
|
274 |
state = SROAD; |
|
275 |
break; |
|
276 |
} |
|
277 |
} |
|
278 |
break; |
|
279 |
case SHIGHWAY:/*On highway*/ |
|
280 |
#ifdef ORB_INTERSECTION |
|
281 |
orb1_set_color(CYAN); |
|
282 |
#endif |
|
283 |
while(!button1_click()){ |
|
284 |
highwayFSM(); |
|
285 |
} |
|
286 |
state = SINTERSECTION_ENTER; |
|
287 |
break; |
|
288 |
default: |
|
289 |
usb_puts("I got stuck in an unknown state! My state is "); |
|
290 |
usb_puti(state); |
|
291 |
} |
|
292 |
} |
|
293 |
|
|
294 |
} |
|
295 |
|
|
296 |
int wirelessPacketHandle(int state){ |
|
297 |
int dataLength = 0; |
|
298 |
unsigned char *packet = NULL; |
|
299 |
packet = wl_basic_do_default(&dataLength); |
|
300 |
|
|
301 |
/* sanity check */ |
|
302 |
if(dataLength == 0 || packet == NULL) //no packet |
|
303 |
return ENOPACKET; |
|
304 |
|
|
305 |
if(dataLength != PACKET_LENGTH) |
|
306 |
return EPACKETLEN; |
|
307 |
|
|
308 |
usb_puts("Recieved Wireless Packet: "); |
|
309 |
for(int i = 0; i < dataLength; i++){ |
|
310 |
usb_puti(packet[i]); |
|
311 |
usb_putc(' '); |
|
312 |
} |
|
313 |
usb_putc('\n'); |
|
314 |
|
|
315 |
switch (packet[0]){ |
|
316 |
case WROADENTRY: //[type, bot, road] |
|
317 |
return ENOACTION; |
|
318 |
break; |
|
319 |
case WROADEXIT: //[type, bot, road] |
|
320 |
return ENOACTION; |
|
321 |
break; |
|
322 |
case WROADSTOP: //[type, bot, road] |
|
323 |
return ENOACTION; |
|
324 |
break; |
|
325 |
case WINTERSECTIONENTRY: //[type, bot, intersection, fromDir, toDir] |
|
326 |
if (packet[2] == intersectionNum){ |
|
327 |
switch (state){ |
|
328 |
case SINTERSECTION_ENTER: |
|
329 |
sendResolv(false); |
|
330 |
resolvPrevBotID = -2; |
|
331 |
return KRESOLVINGENTER; |
|
332 |
break; |
|
333 |
case SINTERSECTION_ENTER_RESOLV: |
|
334 |
return ENOACTION; |
|
335 |
case SINTERSECTION_WAIT: |
|
336 |
case SINTERSECTION_DRIVE: |
|
337 |
if(queueNextBot == (char) -1){ |
|
338 |
sendBuffer[0] = WINTERSECTIONREPLY; |
|
339 |
sendBuffer[2] = intersectionNum; |
|
340 |
sendBuffer[3] = packet[1]; |
|
341 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
342 |
queueNextBot = packet[1]; |
|
343 |
return KREPLIEDTOENTER; |
|
344 |
} |
|
345 |
break; |
|
346 |
|
|
347 |
} |
|
348 |
} |
|
349 |
break; |
|
350 |
case WINTERSECTIONREPLY: //[type, fromBot, intersection, toBot] |
|
351 |
if (packet[2] == intersectionNum){ |
|
352 |
switch (state){ |
|
353 |
case SINTERSECTION_ENTER: |
|
354 |
case SINTERSECTION_ENTER_RESOLV: |
|
355 |
if(packet[3] == id){ //Reply for me |
|
356 |
queuePrevBot = packet[1]; |
|
357 |
return KPLACEDINQUEUE; |
|
358 |
} else { |
|
359 |
if(packet[3] == resolvPrevBotID) |
|
360 |
resolvPrevBotID = -1; |
|
361 |
return KFAILEDTOQUEUE; |
|
362 |
} |
|
363 |
break; |
|
364 |
default: |
|
365 |
return ENOACTION; |
|
366 |
} |
|
367 |
} |
|
368 |
break; |
|
369 |
case WINTERSECTIONEXIT: //[type, bot, intersection] |
|
370 |
if (packet[2] == intersectionNum){ |
|
371 |
switch (state){ |
|
372 |
case SINTERSECTION_WAIT: |
|
373 |
if(packet[1]==queuePrevBot){ |
|
374 |
queuePrevBot=-1; |
|
375 |
return KFIRSTINQUEUE; |
|
376 |
} |
|
377 |
} |
|
378 |
} |
|
379 |
break; |
|
380 |
case WINTERSECTIONGO: //[type, bot, intersection] |
|
381 |
break; |
|
382 |
case WINTERSECTIONPOLICEENTRY: //[?] |
|
383 |
return ENOACTION; |
|
384 |
break; |
|
385 |
case WINTERSECTIONRESOLVERACE: //[type, bot, intersection, num] |
|
386 |
//in the case robots draw the same number these will be |
|
387 |
//arbitrated using the sending robot's id, prefering |
|
388 |
//lower ids |
|
389 |
usb_puts("Now in wireless WINTERSECTIONRESOLVERACE handler: resolvPrevBotID: "); |
|
390 |
usb_puti((int) resolvPrevBotID); |
|
391 |
usb_putc('\n'); |
|
392 |
if (packet[2] == intersectionNum){ |
|
393 |
switch (state){ |
|
394 |
case SINTERSECTION_ENTER: |
|
395 |
case SINTERSECTION_ENTER_RESOLV: |
|
396 |
if(resolvPrevBotID == (char) -3){ |
|
397 |
usb_puts("resolvPrevBotID == -3; sending a resolv packet and setting to -2\n"); |
|
398 |
sendResolv(false); |
|
399 |
resolvPrevBotID = -2; |
|
400 |
} |
|
401 |
if((unsigned char) packet[3] == resolvDraw && id > packet[1]){ |
|
402 |
//other bot drew same number as me, |
|
403 |
//and i have a higher id, so it goes first. |
|
404 |
usb_puts("bot "); |
|
405 |
usb_puti(packet[1]); |
|
406 |
usb_puts(" Drew the same number as me and I have a higher id, so it goes first\n"); |
|
407 |
resolvPrevBotID = packet[1]; |
|
408 |
} else if((unsigned char) packet[3] == resolvPrevBotDraw && resolvPrevBotID > packet[1]){ |
|
409 |
//other bot drew same number as the bot before me, |
|
410 |
//so if it has a higher id than the bot before me, |
|
411 |
//it is going to go in between that one and me. |
|
412 |
usb_puts("bot "); |
|
413 |
usb_puti(packet[1]); |
|
414 |
usb_puts(" Drew the same number as the bot before me, and the bot before me has have a higher id, so this goes first\n"); |
|
415 |
resolvPrevBotID = packet[1]; |
|
416 |
} else if((unsigned char)packet[3] < resolvDraw && (unsigned char)packet[3] > resolvPrevBotDraw){ |
|
417 |
//found a bot that goes in between the bot before me |
|
418 |
//and me, so now it is before me. |
|
419 |
usb_puts("bot "); |
|
420 |
usb_puti(packet[1]); |
|
421 |
usb_puts(" Drew a lower number bot before me, and the bot before me has have a higher id, so this goes first\n"); |
|
422 |
resolvPrevBotDraw = packet[3]; |
|
423 |
resolvPrevBotID = packet[1]; |
|
424 |
} |
|
425 |
return KRESOLVINGENTER; |
|
426 |
break; |
|
427 |
case SINTERSECTION_WAIT: |
|
428 |
case SINTERSECTION_DRIVE: |
|
429 |
usb_puts("Trying to resolv in non resolv case...queueNextbot = "); usb_puti(queueNextBot); usb_puts("\n"); |
|
430 |
if(queueNextBot == (char) -1){ |
|
431 |
sendResolv(true); |
|
432 |
} |
|
433 |
return KRESOLVINGENTER; |
|
434 |
break; |
|
435 |
} |
|
436 |
|
|
437 |
} |
|
438 |
break; |
|
439 |
case WHIGHWAYENTRY: //[type, bot, highway] |
|
440 |
return ENOACTION; |
|
441 |
break; |
|
442 |
case WHIGHWAYREPLY: //[type, fromBot, highway, toBot] |
|
443 |
return ENOACTION; |
|
444 |
break; |
|
445 |
case WHIGHWAYEXIT: //[type, bot, highway] |
|
446 |
return ENOACTION; |
|
447 |
break; |
|
448 |
case WPINGGLOBAL: //[type, bot] |
|
449 |
return ENOACTION; |
|
450 |
break; |
|
451 |
case WPINGBOT: //[type, fromBot, toBot] |
|
452 |
return ENOACTION; |
|
453 |
break; |
|
454 |
case WPINGQUEUE: //[type, fromBot, toBot] |
|
455 |
return ENOACTION; |
|
456 |
break; |
|
457 |
case WPINGREPLY: //[type, fromBot, toBot] |
|
458 |
return ENOACTION; |
|
459 |
break; |
|
460 |
case WCOLLISIONAVOID: //[type, bot, intersection, collision-int] //Note: collision is an int and thus takes two spaces |
|
461 |
return ENOACTION; |
|
462 |
break; |
|
463 |
default: |
|
464 |
return ENOACTION; |
|
465 |
break; |
|
466 |
} |
|
467 |
} |
|
468 |
|
|
469 |
unsigned char resolvRandomNumberGen(){ |
|
470 |
if ((resolvSeed *= (rtc_get())%9) == 0){ |
|
471 |
return resolvSeed + 1; //0 is a reseved priority value for the last |
|
472 |
//bot that is already in the queue. |
|
473 |
} |
|
474 |
return resolvSeed; |
|
475 |
} |
|
476 |
void sendResolv(bool override){ |
|
477 |
if(!override) |
|
478 |
resolvDraw = resolvRandomNumberGen(); |
|
479 |
else |
|
480 |
resolvDraw = 0; |
|
481 |
sendBuffer[0] = WINTERSECTIONRESOLVERACE; |
|
482 |
sendBuffer[2] = intersectionNum; |
|
483 |
sendBuffer[3] = resolvDraw; |
|
484 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
485 |
} |
|
486 |
void enterIntersection(void){ |
|
487 |
//Sends packet announcing its entry to the intersection |
|
488 |
sendBuffer[0] = WINTERSECTIONENTRY; |
|
489 |
sendBuffer[2] = intersectionNum;//Intersection # |
|
490 |
sendBuffer[3] = 0; //getIntersectPos(sign); |
|
491 |
sendBuffer[4] = turnDir; |
|
492 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
493 |
} |
|
494 |
#endif |
trunk/code/projects/traffic_navigation/highways.c | ||
---|---|---|
46 | 46 |
usb_puts(" : "); |
47 | 47 |
usb_puti(averagei); |
48 | 48 |
usb_puts("\r\n"); |
49 |
#ifdef ORB_HIGHWAY |
|
49 | 50 |
orb1_set_color(CYAN); |
51 |
#endif |
|
50 | 52 |
if(range == -1){ |
51 | 53 |
range = PASS_DISTANCE+50; |
52 | 54 |
} |
... | ... | |
66 | 68 |
case 2: |
67 | 69 |
if(changeLanes()){ |
68 | 70 |
states = 0; |
71 |
#ifdef ORB_HIGHWAY |
|
69 | 72 |
orb1_set_color(GREEN); |
73 |
#endif |
|
70 | 74 |
} |
75 |
#ifdef ORB_HIGHWAY |
|
71 | 76 |
else orb1_set_color(YELLOW); |
77 |
#endif |
|
72 | 78 |
break; |
73 | 79 |
} |
74 | 80 |
return 0; |
... | ... | |
90 | 96 |
average += range; |
91 | 97 |
if(count >= COUNT_MAX){ |
92 | 98 |
if(state[0] != MERGELEFT){ |
99 |
#ifdef ORB_HIGHWAY |
|
93 | 100 |
orb1_set_color(ORANGE); |
101 |
#endif |
|
94 | 102 |
if(average / COUNT_MAX < PASS_DISTANCE){ |
95 | 103 |
merge(ILEFT); |
96 | 104 |
usb_puts("MERGE \r\n"); |
... | ... | |
100 | 108 |
count =0; |
101 | 109 |
average = 0; |
102 | 110 |
} |
111 |
#ifdef ORB_HIGHWAY |
|
103 | 112 |
//else orb1_set_color(GREEN); |
113 |
#endif |
|
104 | 114 |
} |
105 | 115 |
*/ |
Also available in: Unified diff