root / trunk / code / projects / traffic_navigation / main-intersectionDebug.c @ 1991
History | View | Annotate | Download (11.7 KB)
1 | 1969 | azl | #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 |