Revision 1979
Added old files that used to compile,
new main in sendGraph that sends a graph packet - temporary
trunk/code/projects/traffic_navigation/main-new.cold | ||
---|---|---|
1 |
/* |
|
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 |
#ifdef MAIN_NEW |
|
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 |
bool done; |
|
16 |
|
|
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 |
orb1_set_color(GREEN); |
|
37 |
delay_ms(1000); |
|
38 |
orb1_set_color(ORB_OFF); |
|
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 |
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 |
orb1_set_color(WHITE); |
|
63 |
orb2_set_color(ORB_OFF); |
|
64 |
#endif |
|
65 |
start(); |
|
66 |
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 |
} |
|
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 |
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 |
|
|
118 |
enterIntersection(); //sends wireless packet for entry |
|
119 |
state = SINTERSECTION_WAIT; |
|
120 |
|
|
121 |
rtc_reset(); //reset rtc for timeout wait for reply |
|
122 |
done = false; |
|
123 |
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 |
orb1_set_color(PURPLE); |
|
165 |
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 |
turn(getIntersectType(sign), turnDir); |
|
263 |
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 |
/* |
|
291 |
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 |
}*/ |
|
303 |
state = SROAD; |
|
304 |
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 |
trunk/code/projects/traffic_navigation/sendGraph.c | ||
---|---|---|
1 |
#include <dragonfly_lib.h> |
|
2 |
#include <wl_basic.h> |
|
3 |
#include "sendGraph.h" |
|
4 |
|
|
5 |
int main(void){ |
|
6 |
|
|
7 |
dragonfly_init(ALL_ON); |
|
8 |
wl_basic_init_default(); |
|
9 |
wl_set_channel(12); |
|
10 |
|
|
11 |
node a; |
|
12 |
node b; |
|
13 |
|
|
14 |
edge aTob; |
|
15 |
edge bToa; |
|
16 |
|
|
17 |
//Create the edges |
|
18 |
aTob.to = 'b'; |
|
19 |
aTob.dist = 25; |
|
20 |
|
|
21 |
bToa.to = 'a'; |
|
22 |
bToa.dist = 25; |
|
23 |
|
|
24 |
|
|
25 |
a.outgoingEdges[0] = aTob; |
|
26 |
b.outgoingEdges[0] = bToa; |
|
27 |
|
|
28 |
|
|
29 |
a.type = 1; |
|
30 |
a.intNum = 1; |
|
31 |
a.numOut = 1; |
|
32 |
a.outSeen = 1; |
|
33 |
|
|
34 |
|
|
35 |
b.type = 1; |
|
36 |
b.intNum = 2; |
|
37 |
b.numOut = 1; |
|
38 |
b.outSeen = 1; |
|
39 |
|
|
40 |
intersections[0] = a; |
|
41 |
intersections[1] = b; |
|
42 |
|
|
43 |
sendIntersectionGraph(); |
|
44 |
} |
|
45 |
|
|
46 |
|
|
47 |
|
|
48 |
|
|
49 |
|
|
50 |
/* |
|
51 |
* Uses wireless to send the graph sructure |
|
52 |
* to other robots |
|
53 |
* |
|
54 |
* @return |
|
55 |
* |
|
56 |
*/ |
|
57 |
void sendIntersectionGraph() { |
|
58 |
int i; |
|
59 |
node_union graph; |
|
60 |
|
|
61 |
for(i=0; i< NUM_FEATURES; i++){ |
|
62 |
graph.n = intersections[i]; |
|
63 |
wl_basic_send_global_packet(42, graph.array, 12); |
|
64 |
wl_basic_send_global_packet(42, '\n', 1); |
|
65 |
} |
|
66 |
} |
trunk/code/projects/traffic_navigation/sendGraph.h | ||
---|---|---|
1 |
#include "intersectData.h" |
|
2 |
|
|
3 |
/* Data is sent in the following order |
|
4 |
* INT0_NUM OUTEDGE_0 OUTEDGE_1 ... \n |
|
5 |
* INT1_NUM OUTEDGE_0 ... \n |
|
6 |
* ... |
|
7 |
* ... |
|
8 |
*/ |
|
9 |
void sendIntersectionGraph(); |
|
10 |
|
|
11 |
|
|
12 |
/* Representation of an edge (road) */ |
|
13 |
typedef struct { |
|
14 |
char to; /* Where does this edge lead to? */ |
|
15 |
char dist; /* Where does it come from? */ |
|
16 |
}edge; |
|
17 |
|
|
18 |
/* Representation of an intersection on the graph */ |
|
19 |
typedef struct { |
|
20 |
char type; /* Note that there are at most 5 intersection types */ |
|
21 |
char intNum; /* What is the intersection number */ |
|
22 |
char numOut; /* Note that we can have no more than 4 outgoing edges */ |
|
23 |
char outSeen; /* The number of the outgoing edges that we have seen for this intersection */ |
|
24 |
edge outgoingEdges [4]; |
|
25 |
}node; |
|
26 |
|
|
27 |
/* A union that is used in wireless transmissions */ |
|
28 |
typedef union { |
|
29 |
node n; |
|
30 |
char array[12]; |
|
31 |
}node_union; |
|
32 |
|
|
33 |
/* This array holds all of the intersections that are represented in the graph |
|
34 |
* after its creation, the graph is transmitted wirelessly */ |
|
35 |
node intersections [NUM_FEATURES]; |
trunk/code/projects/traffic_navigation/Makefile | ||
---|---|---|
11 | 11 |
USE_WIRELESS = 1 |
12 | 12 |
|
13 | 13 |
# com1 = serial port. Use lpt1 to connect to parallel port. |
14 |
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
|
|
14 |
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/tty.usbserial*'; fi)
|
|
15 | 15 |
|
16 | 16 |
else |
17 | 17 |
COLONYROOT := ../$(COLONYROOT) |
trunk/code/projects/traffic_navigation/mapping.hold | ||
---|---|---|
1 |
#include "intersectData.h" |
|
2 |
|
|
3 |
/* Data is sent in the following order |
|
4 |
* INT0_NUM OUTEDGE_0 OUTEDGE_1 ... \n |
|
5 |
* INT1_NUM OUTEDGE_0 ... \n |
|
6 |
* ... |
|
7 |
* ... |
|
8 |
*/ |
|
9 |
|
|
10 |
/* Representation of an edge (road) */ |
|
11 |
typedef struct { |
|
12 |
char to; /* Where does this edge lead to? */ |
|
13 |
char dist; /* Where does it come from? */ |
|
14 |
}edge |
|
15 |
|
|
16 |
/* Representation of an intersection on the graph */ |
|
17 |
typedef struct { |
|
18 |
char type; /* Note that there are at most 5 intersection types */ |
|
19 |
char intNum; /* What is the intersection number */ |
|
20 |
char numOut; /* Note that we can have no more than 4 outgoing edges */ |
|
21 |
char outSeen; /* The number of the outgoing edges that we have seen for this intersection */ |
|
22 |
edge[4] outgoingEdges; |
|
23 |
}node |
|
24 |
|
|
25 |
/* A union that is used in wireless transmissions */ |
|
26 |
typedef union { |
|
27 |
node n; |
|
28 |
char array[12]; |
|
29 |
}node_union |
|
30 |
|
|
31 |
/* This array holds all of the intersections that are represented in the graph |
|
32 |
* after its creation, the graph is transmitted wirelessly */ |
|
33 |
node[NUM_FEATURES] intersections; |
|
34 |
|
|
35 |
int createEdge(edge* newEdge); |
trunk/code/projects/traffic_navigation/main.cold | ||
---|---|---|
1 |
/* |
|
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 |
#ifndef MAIN_NEW |
|
11 |
#ifndef WIRELESSWATCH |
|
12 |
|
|
13 |
static int state, sign, dataLength, pingWaitTime, turnDir; |
|
14 |
static char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, nextDir, nextPath, intersectionNum; |
|
15 |
static unsigned char *packet; |
|
16 |
|
|
17 |
void enterIntersectionQueue(int sign); |
|
18 |
void waitInIntersectionQueue(void); |
|
19 |
void driveThroughIntersection(int sign); |
|
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 |
/* while(1) |
|
46 |
doDrive(255); |
|
47 |
*/ |
|
48 |
|
|
49 |
while (1) { |
|
50 |
/*DTM Finite State Machine*/ |
|
51 |
switch(state){ |
|
52 |
case SROAD:/*Following a normal road*/ |
|
53 |
// sign = lineFollow(); |
|
54 |
//other road behaviors |
|
55 |
//tailgating? |
|
56 |
//read barcode |
|
57 |
sign = doDrive(200); |
|
58 |
if(button1_click()){ |
|
59 |
state = SINTERSECTION; |
|
60 |
} |
|
61 |
break; |
|
62 |
case SINTERSECTION:/*Entering, and in intersection*/ |
|
63 |
intersectionNum = 0; |
|
64 |
/*Intersection queue: |
|
65 |
*Each robot when entering the intersection will check for other robots |
|
66 |
*in the intersection, and insert itself in a queue to go through. |
|
67 |
*/ |
|
68 |
prevBot = 0; |
|
69 |
nextBot = 0; |
|
70 |
|
|
71 |
sign = 0; //Test code until barcodes integrated |
|
72 |
|
|
73 |
enterIntersectionQueue(sign); |
|
74 |
|
|
75 |
orb1_set_color(PURPLE); |
|
76 |
//waits for its turn |
|
77 |
|
|
78 |
waitInIntersectionQueue(); |
|
79 |
|
|
80 |
orb1_set_color(RED); |
|
81 |
//Drives through intersection |
|
82 |
driveThroughIntersection(sign); |
|
83 |
|
|
84 |
//Exits intersection |
|
85 |
sendBuffer[0] = WINTERSECTIONEXIT; |
|
86 |
sendBuffer[2] = intersectionNum;//Intersection # |
|
87 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
88 |
orb1_set_color(ORANGE); |
|
89 |
while(1){ |
|
90 |
stop(); |
|
91 |
sign = doDrive(200); |
|
92 |
if(button1_click()){ |
|
93 |
start(); |
|
94 |
state = SHIGHWAY; |
|
95 |
break; |
|
96 |
} |
|
97 |
if(button2_click()){ |
|
98 |
start(); |
|
99 |
state = SROAD; |
|
100 |
break; |
|
101 |
} |
|
102 |
} |
|
103 |
break; |
|
104 |
case SHIGHWAY:/*On highway*/ |
|
105 |
orb1_set_color(CYAN); |
|
106 |
while(!button1_click()){ |
|
107 |
highwayFSM(); |
|
108 |
} |
|
109 |
state = SINTERSECTION; |
|
110 |
break; |
|
111 |
default: |
|
112 |
usb_puts("I got stuck in an unknown state! My state is "); |
|
113 |
usb_puti(state); |
|
114 |
} |
|
115 |
} |
|
116 |
|
|
117 |
} |
|
118 |
|
|
119 |
void enterIntersectionQueue(int sign){ |
|
120 |
|
|
121 |
//Choose turn direction |
|
122 |
intersectionNum = getIntersectType(sign); |
|
123 |
turnDir = validateTurn(sign, getTurnType(4)); |
|
124 |
//Sends packet announcing its entry to the intersection |
|
125 |
sendBuffer[0] = WINTERSECTIONENTRY; |
|
126 |
sendBuffer[2] = intersectionNum;//Intersection # |
|
127 |
sendBuffer[3] = getIntersectPos(sign); |
|
128 |
sendBuffer[4] = turnDir; |
|
129 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
130 |
orb1_set_color(BLUE); |
|
131 |
stop(); |
|
132 |
doDrive(0); |
|
133 |
rtc_reset(); |
|
134 |
while(rtc_get() < 8){//waits for a reply, otherwise assumes it is first in queue |
|
135 |
dataLength = 0; |
|
136 |
packet = NULL; |
|
137 |
packet = wl_basic_do_default(&dataLength); |
|
138 |
if(packet && dataLength==PACKET_LENGTH && packet[2]==intersectionNum){ |
|
139 |
if(packet[0] == WINTERSECTIONREPLY){ |
|
140 |
if(packet[3] == id){//Reply for me |
|
141 |
prevBot = packet[1]; |
|
142 |
orb2_set_color(GREEN); |
|
143 |
break; |
|
144 |
}else if(packet[1] != id){//Someone else got here first, try again |
|
145 |
sendBuffer[0] = WINTERSECTIONENTRY; |
|
146 |
sendBuffer[2] = intersectionNum; |
|
147 |
sendBuffer[3] = getIntersectPos(sign); |
|
148 |
sendBuffer[4] = turnDir; |
|
149 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
150 |
orb2_set_color(ORANGE); |
|
151 |
rtc_reset(); |
|
152 |
} |
|
153 |
}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0 && prevBot!=packet[1]){ |
|
154 |
sendBuffer[0] = WINTERSECTIONREPLY; |
|
155 |
sendBuffer[2] = intersectionNum; |
|
156 |
sendBuffer[3] = packet[1]; |
|
157 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
158 |
nextBot = packet[1]; |
|
159 |
nextDir = packet[3]; |
|
160 |
nextPath = packet[4]; |
|
161 |
orb2_set_color(BLUE); |
|
162 |
//delay_ms(200); |
|
163 |
} |
|
164 |
delay_ms(0); |
|
165 |
} |
|
166 |
} |
|
167 |
} |
|
168 |
|
|
169 |
void waitInIntersectionQueue(){ |
|
170 |
while(prevBot != 0){ |
|
171 |
dataLength = 0; |
|
172 |
packet = NULL; |
|
173 |
packet = wl_basic_do_default(&dataLength); |
|
174 |
if(packet && dataLength==PACKET_LENGTH){ |
|
175 |
if(packet[2] == intersectionNum){ |
|
176 |
if(packet[1]==prevBot && (packet[0]==WINTERSECTIONGO || packet[0]==WINTERSECTIONEXIT)){ |
|
177 |
prevBot = 0; |
|
178 |
orb2_set_color(PURPLE); |
|
179 |
}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){ |
|
180 |
sendBuffer[0] = WINTERSECTIONREPLY; |
|
181 |
sendBuffer[2] = intersectionNum; |
|
182 |
sendBuffer[3] = packet[1]; |
|
183 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
184 |
nextBot = packet[1]; |
|
185 |
nextDir = packet[3]; |
|
186 |
nextPath = packet[4]; |
|
187 |
orb2_set_color(BLUE); |
|
188 |
} |
|
189 |
} |
|
190 |
/* |
|
191 |
if(ISPING(packet)){ |
|
192 |
sendBuffer[0] = WPINGREPLY; |
|
193 |
sendBuffer[2] = packet[1]; |
|
194 |
if(packet[0]==WPINGQUEUE && packet[3]==nextBot){ |
|
195 |
nextBot = packet[1]; |
|
196 |
} |
|
197 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
198 |
} |
|
199 |
if(packet[0]==WPINGREPLY && packet[2]==id){ |
|
200 |
prevBot = packet[1]; |
|
201 |
pingWaitTime = rtc_get(); |
|
202 |
} |
|
203 |
*/ |
|
204 |
} |
|
205 |
|
|
206 |
if(prevBot && rtc_get() << 12 == 0){ |
|
207 |
sendBuffer[0] = WPINGBOT; |
|
208 |
sendBuffer[2] = prevBot; |
|
209 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
210 |
pingWaitTime = rtc_get(); |
|
211 |
} |
|
212 |
if(prevBot && pingWaitTime - rtc_get() > 4){ |
|
213 |
sendBuffer[0] = WPINGBOT; |
|
214 |
if(pingWaitTime - rtc_get() > 8){ |
|
215 |
sendBuffer[0] = WPINGQUEUE; |
|
216 |
} |
|
217 |
sendBuffer[2] = prevBot; |
|
218 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
219 |
} |
|
220 |
/* if(prevBot && pingWaitTime - rtc_get() > 12){ |
|
221 |
prevBot = 0; |
|
222 |
} |
|
223 |
*/ |
|
224 |
} |
|
225 |
} |
|
226 |
|
|
227 |
void driveThroughIntersection(int sign){ |
|
228 |
//Code to choose path through intersection goes in this while loop |
|
229 |
//But here's the code to handle wireless while in the intersection... |
|
230 |
start(); |
|
231 |
turn(getIntersectType(0), turnDir); |
|
232 |
while(doDrive(200) != FINISHED){ |
|
233 |
dataLength = 0; |
|
234 |
packet = NULL; |
|
235 |
packet = wl_basic_do_default(&dataLength); |
|
236 |
if(dataLength==PACKET_LENGTH){ |
|
237 |
if(packet[2]==intersectionNum/*Intersection Num*/){ |
|
238 |
if(packet[0]==WINTERSECTIONEXIT){ |
|
239 |
prevBot = 0; |
|
240 |
orb2_set_color(RED); |
|
241 |
}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){ |
|
242 |
sendBuffer[0] = WINTERSECTIONREPLY; |
|
243 |
sendBuffer[2] = intersectionNum;//Intersection # |
|
244 |
sendBuffer[3] = packet[1]; |
|
245 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
246 |
nextBot = packet[1]; |
|
247 |
nextDir = packet[3]; |
|
248 |
nextPath = packet[4]; |
|
249 |
orb2_set_color(YELLOW); |
|
250 |
} |
|
251 |
} |
|
252 |
if(ISPING(packet)){ |
|
253 |
sendBuffer[0] = WPINGREPLY; |
|
254 |
sendBuffer[2] = packet[1]; |
|
255 |
if(packet[0]==WPINGQUEUE && packet[3]==nextBot){ |
|
256 |
nextBot = packet[1]; |
|
257 |
} |
|
258 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
259 |
} |
|
260 |
} |
|
261 |
if(prevBot==0 && nextBot!=0){//let 2 bots go through at same time |
|
262 |
/*if(bots paths won't collide){ |
|
263 |
sendBuffer[0] = WINTERSECTIONGO; |
|
264 |
sendBuffer[2] = 0;//Intersection # |
|
265 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
266 |
*/ |
|
267 |
} |
|
268 |
} |
|
269 |
} |
|
270 |
#endif |
|
271 |
#endif |
trunk/code/projects/traffic_navigation/mapping.cold | ||
---|---|---|
1 |
#include "" |
|
2 |
#include "mapping.h" |
|
3 |
#include "lineDrive.h" |
|
4 |
#include <dragonfly_lib.h> |
|
5 |
|
|
6 |
//The last intersection that we encountered |
|
7 |
int lastInt; |
|
8 |
|
|
9 |
|
|
10 |
/* |
|
11 |
* Traverses the map using DFS |
|
12 |
* Returns 0 if all of the intersections in the database were seen |
|
13 |
* 1 otherwise |
|
14 |
*/ |
|
15 |
int createEdge(edge* newEdge){ |
|
16 |
char barcode; |
|
17 |
char time; |
|
18 |
rtc_init(SIXTEENTH_SECOND, NULL); |
|
19 |
rtc_reset(); |
|
20 |
while(barcode = NOBARCODE){ |
|
21 |
barcode = (char) doDrive(200); |
|
22 |
} |
|
23 |
time = rtc_get(); |
|
24 |
newEdge.to = barcode; |
|
25 |
newEdge.dist = time; |
|
26 |
return 0; |
|
27 |
} |
|
28 |
|
|
29 |
int createMap(){ |
|
30 |
|
|
31 |
int seenout_count = 0; /* The number of intersections we have completely seen */ |
|
32 |
//int hist_count = 0; /* The size of our history stack */ |
|
33 |
|
|
34 |
char[NUM_FEATURES] seen; /* Have we seen this node? */ |
|
35 |
char[NUM_FEATURES] seenout; /* Have we seen the outoging edges of this node? */ |
|
36 |
//char[NUM_FEATURES] history; /* A stack representing the history */ |
|
37 |
|
|
38 |
int i; |
|
39 |
int outEdges, currInt, chosenEdge; |
|
40 |
|
|
41 |
|
|
42 |
/* Initialize all of these arrays */ |
|
43 |
for(i = 0; i<NUM_FEATURES; i++) { |
|
44 |
seen[i] = 0; |
|
45 |
seenout[i] = 0; |
|
46 |
history[i] = 0; |
|
47 |
intersections[i].type = 0; |
|
48 |
intersections[i].intNum = 0; |
|
49 |
intersections[i].numOut = 0; |
|
50 |
intersections[i].outSeen = 0; |
|
51 |
intersections[i].outgoingEdges[0].to = 0; |
|
52 |
intersections[i].outgoingEdges[0].dist = 0; |
|
53 |
intersections[i].outgoingEdges[1].to = 0; |
|
54 |
intersections[i].outgoingEdges[1].dist = 0; |
|
55 |
intersections[i].outgoingEdges[2].to = 0; |
|
56 |
intersections[i].outgoingEdges[2].dist = 0; |
|
57 |
intersections[i].outgoingEdges[3].to = 0; |
|
58 |
intersections[i].outgoingEdges[3].dist = 0; |
|
59 |
} |
|
60 |
|
|
61 |
|
|
62 |
/* Drives to the nearest intersection */ |
|
63 |
barcode = nextInt(); |
|
64 |
|
|
65 |
while(seenout_count < NUM_FEATURES) |
|
66 |
{ |
|
67 |
currInt = getIntersectionNum(barcode); |
|
68 |
seen[currInt] = 1; |
|
69 |
|
|
70 |
/* Get the number of outgoing edges */ |
|
71 |
outEdges = getNumOut(getIntersectType(currInt)); |
|
72 |
|
|
73 |
/* Randomly choose an outgoing edge that we have not seen to go down |
|
74 |
* if no such edge exists */ |
|
75 |
chosenEdge = rand() % outEdges; |
|
76 |
|
|
77 |
/* We have not seen all the outgoing edges */ |
|
78 |
if(!seenout[currInt]){ |
|
79 |
intersection[currInt].outSeen++; |
|
80 |
/* We have finished seeing all outgoing edges of the intersection */ |
|
81 |
if(intersections[currInt].numOut == intersection[currInt].outSeen){ |
|
82 |
seenout[currInt] = 1; |
|
83 |
seenout_count ++; |
|
84 |
} |
|
85 |
} |
|
86 |
/* Traverses edge, stores information in struct */ |
|
87 |
traverseEdge(chosenEdge, &(intersection[currInt].outgoingEdges[chosenEdge])); |
|
88 |
} |
|
89 |
|
|
90 |
/* We are done traversing send the graph to the robots */ |
|
91 |
sendIntersectionGraph(); |
|
92 |
|
|
93 |
|
|
94 |
return 0; |
|
95 |
} |
|
96 |
|
|
97 |
/* Given an intersection type, returns the number of outgoing edges */ |
|
98 |
int getNumOut(int type) { |
|
99 |
switch(type){ |
|
100 |
case 0: return; |
|
101 |
case 1: return; |
|
102 |
case 2: return; |
|
103 |
case 3: return; |
|
104 |
case 4: return; |
|
105 |
} |
|
106 |
|
|
107 |
return -1; |
|
108 |
} |
|
109 |
|
|
110 |
|
|
111 |
/* Creates an edge in the graph */ |
|
112 |
int insertEdge(){ |
|
113 |
|
|
114 |
} |
|
115 |
|
|
116 |
|
|
117 |
/* |
|
118 |
* Drives to the next intersection and returns its ID |
|
119 |
* If we are at a dead end, returns -1 |
|
120 |
*/ |
|
121 |
int nextInt(){ |
|
122 |
|
|
123 |
} |
|
124 |
|
|
125 |
|
|
126 |
/* |
|
127 |
* Given an intersection node returns an integer that |
|
128 |
* can be sent wirelessly |
|
129 |
* |
|
130 |
*/ |
|
131 |
int encodeNode(node n){ |
|
132 |
|
|
133 |
|
|
134 |
} |
|
135 |
|
|
136 |
/* |
|
137 |
* Uses wireless to send the graph sructure |
|
138 |
* to other robots |
|
139 |
* |
|
140 |
* @return |
|
141 |
* |
|
142 |
*/ |
|
143 |
void sendIntersectionGraph() { |
|
144 |
int i; |
|
145 |
node_union graph; |
|
146 |
|
|
147 |
for(i=0; i< NUM_FEATURES; i++){ |
|
148 |
graph.n = intersctions[i]; |
|
149 |
wl_basic_send_global(42, graph.array, 12); |
|
150 |
} |
|
151 |
} |
Also available in: Unified diff