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