Index: trunk/code/projects/traffic_navigation/main-new.cold
===================================================================
--- trunk/code/projects/traffic_navigation/main-new.cold	(nonexistent)
+++ trunk/code/projects/traffic_navigation/main-new.cold	(revision 1979)
@@ -0,0 +1,520 @@
+/*
+ * main.c for Traffic Navigation
+ * Runs the highest level behavior for the Dynamic Traffic Navigation (DTM) SURG
+ *
+ * Author: Colony Project, CMU Robotics Club
+ */
+
+#include "traffic_navigation.h"
+#include "../linefollowing/lineDrive.h"
+#ifdef MAIN_NEW
+
+	static int state, sign, turnDir;
+	static char sendBuffer[PACKET_LENGTH], queuePrevBot, queueNextBot, id, nextDir, nextPath, intersectionNum, resolvPrevBotID = -3;
+	unsigned char resolvSeed = 0xC9, resolvDraw = 0, resolvPrevBotDraw = 0;
+	bool done;
+
+	int wirelessPacketHandle(int state);
+	void enterIntersection(void);
+	void sendResolv(bool override);
+	unsigned char resolvRandomNumberGen();
+
+int main (void) {
+	
+	/* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */
+	dragonfly_init(ALL_ON);
+	xbee_init();
+	encoders_init();
+	lineDrive_init();
+	rtc_init(SIXTEENTH_SECOND, NULL);	
+	wl_basic_init_default();
+	wl_set_channel(13);
+	initializeData();
+	
+	id = get_robotid();
+	sign = 0;
+	orb1_set_color(GREEN);
+	delay_ms(1000);
+	orb1_set_color(ORB_OFF);
+	
+	//Test code
+	state = SROAD;
+
+	sendBuffer[1] = id;
+
+	/*
+	doDrive(200);
+	turn(DOUBLE, ILEFT);
+	*/
+	/*
+	while(1)
+		doDrive(255);
+	*/
+	
+	while (1) {
+		/*DTM Finite State Machine*/
+		switch(state){
+		case SROAD:/*Following a normal road*/
+			/* implement other road behaviors?
+			 *    -tailgating
+			 */
+#ifdef ORB_INTERSECTION
+			orb1_set_color(WHITE);
+			orb2_set_color(ORB_OFF);
+#endif
+			start();
+			done = false;
+			while(!done){
+				sign = doDrive(200);
+				switch(sign){
+					case NORMAL:
+					case FINISHED:
+					case LOST:
+					case ERROR:
+						break;
+					default:
+						//we have a barcode!
+						state = SINTERSECTION_ENTER;
+						done = true;
+						break;
+
+				}
+			}
+			break;
+		case SINTERSECTION_ENTER:/*Entering, and in intersection*/
+			stop();
+			doDrive(0);
+#ifdef ORB_INTERSECTION
+			orb1_set_color(RED);
+			orb2_set_color(ORB_OFF);
+#endif
+
+#ifdef DEBUG_INTERSECTION
+			usb_puts("STATE: SINTERSECTION_ENTER\n");
+#endif
+
+			/*Intersection queue:
+			 *Each robot when entering the intersection will check for other robots
+			 *in the intersection, and insert itself in a queue to go through.
+			 */
+			queuePrevBot = -1; //the bot that will drive before this bot
+			queueNextBot = -1; //the bot that will drive after this bot
+			resolvPrevBotID = -3; //in the case of a race, the bot that is
+			                      //to enter the queue before this bot
+			resolvPrevBotDraw = 0; //the random priority number that bot has
+			resolvDraw = 0; //my random priority number
+			
+			intersectionNum = getIntersectNum(sign);
+			if(intersectionNum == (char) -1){ //invalid
+				state = SROAD;
+				break;
+			}
+			turnDir = validateTurn(sign, getTurnType(sign));
+			if(turnDir == (char) -1){ //invalid
+				state = SROAD;
+				break;
+			}
+
+			enterIntersection(); //sends wireless packet for entry
+			state = SINTERSECTION_WAIT;
+
+			rtc_reset(); //reset rtc for timeout wait for reply
+			done = false;
+			char retried = 0;
+			while(rtc_get() < 16 && !done){//waits for a reply, otherwise assumes it is first in queue
+				int ret = wirelessPacketHandle(SINTERSECTION_ENTER);
+				if(rtc_get() > 12 && !retried){//by now all resolvs should be done from bots that arrived earlier...
+#ifdef ORB_INTERSECTION
+					orb2_set_color(PURPLE);
+#endif
+					enterIntersection();
+					retried = 1;
+				}
+				switch (ret) {
+					case KPLACEDINQUEUE:
+#ifdef ORB_INTERSECTION
+						orb2_set_color(GREEN);
+#endif
+						done = true;
+						break;
+					case KFAILEDTOQUEUE:
+#ifdef DEBUG_INTERSECTION
+						usb_puts("Failed to queue\n");
+#endif
+#ifdef ORB_INTERSECTION
+						orb2_set_color(RED);
+#endif
+						enterIntersection();
+						rtc_reset();
+						break;
+					case KRESOLVINGENTER:
+
+#ifdef ORB_INTERSECTION
+						orb2_set_color(ORANGE);
+#endif
+						state = SINTERSECTION_ENTER_RESOLV;
+						done = true;
+						break;
+				}
+			}
+			break;
+
+		case SINTERSECTION_ENTER_RESOLV:
+#ifdef ORB_INTERSECTION
+			orb1_set_color(PURPLE);
+			orb2_set_color(ORB_OFF);
+#endif
+#ifdef DEBUG_INTERSECTION
+			usb_puts("STATE: SINTERSECTION_ENTER_RESOLV\n");
+#endif
+
+			rtc_reset();
+			done = false;
+			retried = 0;
+			while(rtc_get() < 9 && !done){
+				int ret = wirelessPacketHandle(SINTERSECTION_ENTER_RESOLV);
+				switch (ret) {
+					case KRESOLVINGENTER:
+#ifdef ORB_INTERSECTION
+						orb2_set_color(YELLOW);
+#endif
+						break;
+					case KPLACEDINQUEUE:
+#ifdef ORB_INTERSECTION
+						orb2_set_color(GREEN);
+#endif
+						done = true;
+						break;
+					case KFAILEDTOQUEUE:
+						usb_puts("Failed to queue\n");
+						orb2_set_color(RED);
+						enterIntersection();
+						rtc_reset();
+						break;
+				}
+				//if resolvPrevBotID == -1, this indicates that
+				//there was a prevbot before, but it has entered
+				//the queue so it's our turn.
+				if(!done && resolvPrevBotID == (char) -1 && !retried){
+					enterIntersection();
+					rtc_reset();
+					retried = 1;
+				//if resolvPrevBotID == -2, we have been
+				//resolving but never have seen a bot with lower
+				//priority than us. after the 6/16ths sec
+				//timeout, assume we are first.
+				} else if(!done && resolvPrevBotID == (char) -2 && rtc_get() > 6){
+					//send a intersection reply to myself to
+					//trigger other bots to enter queue.
+					sendBuffer[0] = WINTERSECTIONREPLY;
+					sendBuffer[2] = intersectionNum;
+					sendBuffer[3] = id;
+					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
+
+					done = true;
+					break;
+				}
+			}
+			state = SINTERSECTION_WAIT;
+			break;
+		case SINTERSECTION_WAIT:/*Waiting in intersection */
+#ifdef ORB_INTERSECTION
+			orb1_set_color(YELLOW);
+			orb2_set_color(ORB_OFF);
+#endif
+#ifdef DEBUG_INTERSECTION
+			usb_puts("STATE: SINTERSECTION_WAIT\n");
+#endif
+
+			while(queuePrevBot != (char) -1){
+				int ret = wirelessPacketHandle(state);
+				switch (ret){
+					case KFIRSTINQUEUE:
+#ifdef ORB_INTERSECTION
+						orb2_set_color(GREEN);
+#endif
+						state = SINTERSECTION_DRIVE;
+						break;
+					case KREPLIEDTOENTER:
+#ifdef ORB_INTERSECTION
+						orb2_set_color(BLUE);
+#endif
+						break;
+					case KRESOLVINGENTER:
+#ifdef ORB_INTERSECTION
+						orb2_set_color(ORANGE);
+#endif
+						break;
+				}
+			}
+			state = SINTERSECTION_DRIVE;
+			break;
+			
+		case SINTERSECTION_DRIVE:
+#ifdef DEBUG_INTERSECTION
+			usb_puts("STATE: SINTERSECTION_DRIVE\n");
+#endif
+#ifdef ORB_INTERSECTION
+			orb1_set_color(GREEN);
+			orb2_set_color(ORB_OFF);
+#endif
+			start();
+			turn(getIntersectType(sign), turnDir);
+			while(doDrive(200) != FINISHED){
+			//while(!button2_click()){
+				 int ret = wirelessPacketHandle(state);
+				 switch (ret){
+					 case KREPLIEDTOENTER:
+#ifdef ORB_INTERSECTION
+						orb2_set_color(BLUE);
+#endif
+						break;
+					 case KRESOLVINGENTER:
+#ifdef ORB_INTERSECTION
+					 	orb2_set_color(ORANGE);
+#endif
+						break;
+				 }
+			 }
+			
+			//Exits intersection
+#ifdef ORB_INTERSECTION
+			orb1_set_color(WHITE);
+#endif
+
+			sendBuffer[0] = WINTERSECTIONEXIT;
+			sendBuffer[2] = intersectionNum;//Intersection #
+			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
+
+			//Exits intersection
+			/*
+			while(1){
+				if(button1_click()){
+					start();
+					state = SHIGHWAY;
+					break;
+				}
+				if(button2_click()){
+					start();
+					state = SROAD;
+					break;
+				}
+			}*/
+			state = SROAD;
+			break;
+		case SHIGHWAY:/*On highway*/
+#ifdef ORB_INTERSECTION
+			orb1_set_color(CYAN);
+#endif
+			while(!button1_click()){
+				highwayFSM();
+			}
+			state = SINTERSECTION_ENTER;
+			break;
+		default:
+			usb_puts("I got stuck in an unknown state! My state is ");
+			usb_puti(state);
+		}
+	}
+
+}
+
+int wirelessPacketHandle(int state){
+	int dataLength = 0;
+	unsigned char *packet = NULL;
+	packet = wl_basic_do_default(&dataLength);
+	
+	/* sanity check */
+	if(dataLength == 0 || packet == NULL) //no packet
+		return ENOPACKET;
+
+	if(dataLength != PACKET_LENGTH)
+		return EPACKETLEN;
+
+	usb_puts("Recieved Wireless Packet: ");
+	for(int i = 0; i < dataLength; i++){
+		usb_puti(packet[i]);
+		usb_putc(' ');
+	}
+	usb_putc('\n');
+
+	switch (packet[0]){
+		case WROADENTRY: //[type, bot, road]
+			return ENOACTION;
+			break;
+		case WROADEXIT: //[type, bot, road]
+			return ENOACTION;
+			break;
+		case WROADSTOP: //[type, bot, road]
+			return ENOACTION;
+			break;
+		case WINTERSECTIONENTRY: //[type, bot, intersection, fromDir, toDir]
+			if (packet[2] == intersectionNum){
+				switch (state){
+					case SINTERSECTION_ENTER:
+						sendResolv(false);
+						resolvPrevBotID = -2;
+						return KRESOLVINGENTER;
+						break;
+					case SINTERSECTION_ENTER_RESOLV:
+						return ENOACTION;
+					case SINTERSECTION_WAIT:
+					case SINTERSECTION_DRIVE:
+						if(queueNextBot == (char) -1){
+                                        		sendBuffer[0] = WINTERSECTIONREPLY;
+                                        		sendBuffer[2] = intersectionNum;
+                                        		sendBuffer[3] = packet[1];
+                                        		wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
+							queueNextBot = packet[1];
+							return KREPLIEDTOENTER;
+						}
+						break;
+
+				}
+			}
+			break;
+		case WINTERSECTIONREPLY: //[type, fromBot, intersection, toBot]
+			if (packet[2] == intersectionNum){
+				switch (state){
+					case SINTERSECTION_ENTER:
+					case SINTERSECTION_ENTER_RESOLV:
+						if(packet[3] == id){ //Reply for me
+							queuePrevBot = packet[1];
+							return KPLACEDINQUEUE;
+						} else {
+							if(packet[3] == resolvPrevBotID)
+								resolvPrevBotID = -1;
+							return KFAILEDTOQUEUE;
+						}
+						break;
+					default:
+						return ENOACTION;
+				}
+			}
+			break;
+		case WINTERSECTIONEXIT: //[type, bot, intersection]
+			if (packet[2] == intersectionNum){
+				switch (state){
+					case SINTERSECTION_WAIT:
+						if(packet[1]==queuePrevBot){
+							queuePrevBot=-1;
+							return KFIRSTINQUEUE;
+						}
+				}
+			}
+			break;
+		case WINTERSECTIONGO: //[type, bot, intersection]
+			break;
+		case WINTERSECTIONPOLICEENTRY: //[?]
+			return ENOACTION;
+			break;
+		case WINTERSECTIONRESOLVERACE: //[type, bot, intersection, num]
+			//in the case robots draw the same number these will be
+			//arbitrated using the sending robot's id, prefering
+			//lower ids
+			usb_puts("Now in wireless WINTERSECTIONRESOLVERACE handler: resolvPrevBotID: ");
+			usb_puti((int) resolvPrevBotID);
+			usb_putc('\n');
+			if (packet[2] == intersectionNum){
+				switch (state){
+					case SINTERSECTION_ENTER:
+					case SINTERSECTION_ENTER_RESOLV:
+						if(resolvPrevBotID == (char) -3){
+							usb_puts("resolvPrevBotID == -3; sending a resolv packet and setting to -2\n");
+							sendResolv(false);
+							resolvPrevBotID = -2;
+						}
+						if((unsigned char) packet[3] == resolvDraw && id > packet[1]){
+							//other bot drew same number as me,
+							//and i have a higher id, so it goes first.
+							usb_puts("bot ");
+							usb_puti(packet[1]);
+							usb_puts(" Drew the same number as me and I have a higher id, so it goes first\n");
+							resolvPrevBotID = packet[1];
+						} else if((unsigned char) packet[3] == resolvPrevBotDraw && resolvPrevBotID > packet[1]){
+							//other bot drew same number as the bot before me,
+							//so if it has a higher id than the bot before me,
+							//it is going to go in between that one and me.
+							usb_puts("bot ");
+							usb_puti(packet[1]);
+							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");
+							resolvPrevBotID = packet[1];
+						} else if((unsigned char)packet[3] < resolvDraw && (unsigned char)packet[3] > resolvPrevBotDraw){
+							//found a bot that goes in between the bot before me
+							//and me, so now it is before me.
+							usb_puts("bot ");
+							usb_puti(packet[1]);
+							usb_puts(" Drew a lower number bot before me, and the bot before me has have a higher id, so this goes first\n");
+							resolvPrevBotDraw = packet[3];
+							resolvPrevBotID = packet[1];
+						}
+						return KRESOLVINGENTER;
+						break;
+					case SINTERSECTION_WAIT:
+					case SINTERSECTION_DRIVE:
+						usb_puts("Trying to resolv in non resolv case...queueNextbot = "); usb_puti(queueNextBot); usb_puts("\n");
+						if(queueNextBot == (char) -1){
+							sendResolv(true);
+						}
+						return KRESOLVINGENTER;
+						break;
+				}
+				
+			}
+			break;
+		case WHIGHWAYENTRY: //[type, bot, highway]
+			return ENOACTION;
+			break;
+		case WHIGHWAYREPLY: //[type, fromBot, highway, toBot]
+			return ENOACTION;
+			break;
+		case WHIGHWAYEXIT: //[type, bot, highway]
+			return ENOACTION;
+			break;
+		case WPINGGLOBAL: //[type, bot]
+			return ENOACTION;
+			break;
+		case WPINGBOT: //[type, fromBot, toBot]
+			return ENOACTION;
+			break;
+		case WPINGQUEUE: //[type, fromBot, toBot]
+			return ENOACTION;
+			break;
+		case WPINGREPLY: //[type, fromBot, toBot]
+			return ENOACTION;
+			break;
+		case WCOLLISIONAVOID: //[type, bot, intersection, collision-int] //Note: collision is an int and thus takes two spaces
+			return ENOACTION;
+			break;
+		default:
+			return ENOACTION;
+			break;
+	}
+}
+
+unsigned char resolvRandomNumberGen(){
+	if ((resolvSeed *= (rtc_get())%9) == 0){
+		return resolvSeed + 1; //0 is a reseved priority value for the last
+		                 //bot that is already in the queue.
+	}
+	return resolvSeed;
+}
+void sendResolv(bool override){
+	if(!override)
+		resolvDraw = resolvRandomNumberGen();
+	else
+		resolvDraw = 0;
+	sendBuffer[0] = WINTERSECTIONRESOLVERACE;
+	sendBuffer[2] = intersectionNum;
+	sendBuffer[3] = resolvDraw;
+	wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
+}
+void enterIntersection(void){
+	//Sends packet announcing its entry to the intersection
+	sendBuffer[0] = WINTERSECTIONENTRY;
+	sendBuffer[2] = intersectionNum;//Intersection #
+	sendBuffer[3] = 0; //getIntersectPos(sign);
+	sendBuffer[4] = turnDir;
+	wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
+}
+#endif
Index: trunk/code/projects/traffic_navigation/sendGraph.c
===================================================================
--- trunk/code/projects/traffic_navigation/sendGraph.c	(nonexistent)
+++ trunk/code/projects/traffic_navigation/sendGraph.c	(revision 1979)
@@ -0,0 +1,66 @@
+#include <dragonfly_lib.h>
+#include <wl_basic.h>
+#include "sendGraph.h"
+
+int main(void){
+
+    dragonfly_init(ALL_ON);
+    wl_basic_init_default();
+    wl_set_channel(12);
+
+	node a;
+	node b;
+			
+	edge aTob;
+	edge bToa;
+	
+	//Create the edges
+	aTob.to = 'b';
+	aTob.dist = 25;
+	
+	bToa.to = 'a'; 
+	bToa.dist = 25;
+	
+	
+	a.outgoingEdges[0] = aTob;
+	b.outgoingEdges[0] = bToa;
+	
+	
+	a.type = 1;
+	a.intNum = 1;
+	a.numOut = 1;
+	a.outSeen = 1;
+
+	
+	b.type = 1;
+	b.intNum = 2;
+	b.numOut = 1;
+	b.outSeen = 1;
+
+	intersections[0] = a;
+	intersections[1] = b;
+	
+	sendIntersectionGraph();
+}
+
+
+
+
+
+/*
+ * Uses wireless to send the graph sructure
+ * to other robots
+ *
+ * @return 
+ *
+ */
+void sendIntersectionGraph() {
+        int i;
+        node_union graph;
+
+        for(i=0; i< NUM_FEATURES; i++){
+                graph.n = intersections[i];
+                wl_basic_send_global_packet(42, graph.array, 12);
+                wl_basic_send_global_packet(42, '\n', 1);
+        }
+}
Index: trunk/code/projects/traffic_navigation/sendGraph.h
===================================================================
--- trunk/code/projects/traffic_navigation/sendGraph.h	(nonexistent)
+++ trunk/code/projects/traffic_navigation/sendGraph.h	(revision 1979)
@@ -0,0 +1,35 @@
+#include "intersectData.h"
+
+/* Data is sent in the following order
+ * INT0_NUM OUTEDGE_0 OUTEDGE_1 ... \n
+ * INT1_NUM OUTEDGE_0 ... \n
+ * ...
+ * ...
+ */
+void sendIntersectionGraph();
+
+
+/* Representation of an edge (road) */
+typedef struct {
+	char to;	/* Where does this edge lead to? */
+	char dist;	/* Where does it come from?	*/
+}edge;
+
+/* Representation of an intersection on the graph */
+typedef struct {
+	char type; 	/* Note that there are at most 5 intersection types */
+	char intNum;	/* What is the intersection number */
+	char numOut;	/* Note that we can have no more than 4 outgoing edges */
+	char outSeen;	/* The number of the outgoing edges that we have seen for this intersection */
+	edge outgoingEdges [4];  
+}node;
+
+/* A union that is used in wireless transmissions */
+typedef union  {
+	node n;
+	char array[12];
+}node_union;
+
+/* This array holds all of the intersections that are represented in the graph
+ * after its creation, the graph is transmitted wirelessly */
+node intersections [NUM_FEATURES]; 
Index: trunk/code/projects/traffic_navigation/Makefile
===================================================================
--- trunk/code/projects/traffic_navigation/Makefile	(revision 1978)
+++ trunk/code/projects/traffic_navigation/Makefile	(revision 1979)
@@ -11,7 +11,7 @@
 USE_WIRELESS = 1
 
 # com1 = serial port. Use lpt1 to connect to parallel port.
-AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
+AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/tty.usbserial*'; fi)
 
 else
 COLONYROOT := ../$(COLONYROOT)
Index: trunk/code/projects/traffic_navigation/mapping.hold
===================================================================
--- trunk/code/projects/traffic_navigation/mapping.hold	(nonexistent)
+++ trunk/code/projects/traffic_navigation/mapping.hold	(revision 1979)
@@ -0,0 +1,35 @@
+#include "intersectData.h"
+
+/* Data is sent in the following order
+ * INT0_NUM OUTEDGE_0 OUTEDGE_1 ... \n
+ * INT1_NUM OUTEDGE_0 ... \n
+ * ...
+ * ...
+ */
+
+/* Representation of an edge (road) */
+typedef struct {
+	char to;	/* Where does this edge lead to? */
+	char dist;	/* Where does it come from?	*/
+}edge
+
+/* Representation of an intersection on the graph */
+typedef struct {
+	char type; 	/* Note that there are at most 5 intersection types */
+	char intNum;	/* What is the intersection number */
+	char numOut;	/* Note that we can have no more than 4 outgoing edges */
+	char outSeen;	/* The number of the outgoing edges that we have seen for this intersection */
+	edge[4] outgoingEdges;  
+}node
+
+/* A union that is used in wireless transmissions */
+typedef union  {
+	node n;
+	char array[12];
+}node_union
+
+/* This array holds all of the intersections that are represented in the graph
+ * after its creation, the graph is transmitted wirelessly */
+node[NUM_FEATURES] intersections; 
+
+int createEdge(edge* newEdge);
Index: trunk/code/projects/traffic_navigation/main.cold
===================================================================
--- trunk/code/projects/traffic_navigation/main.cold	(nonexistent)
+++ trunk/code/projects/traffic_navigation/main.cold	(revision 1979)
@@ -0,0 +1,271 @@
+/*
+ * main.c for Traffic Navigation
+ * Runs the highest level behavior for the Dynamic Traffic Navigation (DTM) SURG
+ *
+ * Author: Colony Project, CMU Robotics Club
+ */
+
+#include "traffic_navigation.h"
+#include "../linefollowing/lineDrive.h"
+#ifndef MAIN_NEW
+#ifndef WIRELESSWATCH
+
+	static int state, sign, dataLength, pingWaitTime, turnDir;
+	static char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, nextDir, nextPath, intersectionNum;
+	static unsigned char *packet;
+
+	void enterIntersectionQueue(int sign);
+	void waitInIntersectionQueue(void);
+	void driveThroughIntersection(int sign);
+	
+int main (void) {
+	
+	/* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */
+	dragonfly_init(ALL_ON);
+	xbee_init();
+	encoders_init();
+	lineDrive_init();
+	rtc_init(SIXTEENTH_SECOND, NULL);	
+	wl_basic_init_default();
+	wl_set_channel(13);
+	initializeData();
+	
+	id = get_robotid();
+	sign = 0;
+	
+	//Test code
+	state = SROAD;
+
+	sendBuffer[1] = id;
+
+	/*
+	doDrive(200);
+	turn(DOUBLE, ILEFT);
+	*/
+/*	while(1)
+		doDrive(255);
+	*/
+	
+	while (1) {
+		/*DTM Finite State Machine*/
+		switch(state){
+		case SROAD:/*Following a normal road*/
+//			sign = lineFollow();
+			//other road behaviors
+				//tailgating?
+			//read barcode
+			sign = doDrive(200);
+			if(button1_click()){
+				state = SINTERSECTION;
+			}
+			break;
+		case SINTERSECTION:/*Entering, and in intersection*/
+			intersectionNum = 0;
+			/*Intersection queue:
+			 *Each robot when entering the intersection will check for other robots
+			 *in the intersection, and insert itself in a queue to go through.
+			 */
+			prevBot = 0;
+			nextBot = 0;
+			
+			sign = 0; //Test code until barcodes integrated
+			
+			enterIntersectionQueue(sign);
+			
+			orb1_set_color(PURPLE);
+			//waits for its turn
+			
+			waitInIntersectionQueue();
+			
+			orb1_set_color(RED);
+			//Drives through intersection
+			driveThroughIntersection(sign);
+			
+			//Exits intersection
+			sendBuffer[0] = WINTERSECTIONEXIT;
+			sendBuffer[2] = intersectionNum;//Intersection #
+			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
+			orb1_set_color(ORANGE);
+			while(1){
+				stop();
+				sign = doDrive(200);
+				if(button1_click()){
+					start();
+					state = SHIGHWAY;
+					break;
+				}
+				if(button2_click()){
+					start();
+					state = SROAD;
+					break;
+				}
+			}
+			break;
+		case SHIGHWAY:/*On highway*/
+			orb1_set_color(CYAN);
+			while(!button1_click()){
+				highwayFSM();
+			}
+			state = SINTERSECTION;
+			break;
+		default:
+			usb_puts("I got stuck in an unknown state! My state is ");
+			usb_puti(state);
+		}
+	}
+
+}
+
+void enterIntersectionQueue(int sign){
+		
+	//Choose turn direction
+	intersectionNum = getIntersectType(sign);
+	turnDir = validateTurn(sign, getTurnType(4));
+	//Sends packet announcing its entry to the intersection
+	sendBuffer[0] = WINTERSECTIONENTRY;
+	sendBuffer[2] = intersectionNum;//Intersection #
+	sendBuffer[3] = getIntersectPos(sign);
+	sendBuffer[4] = turnDir;
+	wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
+	orb1_set_color(BLUE);
+	stop();
+	doDrive(0);
+	rtc_reset();
+	while(rtc_get() < 8){//waits for a reply, otherwise assumes it is first in queue
+		dataLength = 0;
+		packet = NULL;
+		packet = wl_basic_do_default(&dataLength);
+		if(packet && dataLength==PACKET_LENGTH && packet[2]==intersectionNum){
+			if(packet[0] == WINTERSECTIONREPLY){
+				if(packet[3] == id){//Reply for me
+					prevBot = packet[1];
+					orb2_set_color(GREEN);
+					break;
+				}else if(packet[1] != id){//Someone else got here first, try again
+					sendBuffer[0] = WINTERSECTIONENTRY;
+					sendBuffer[2] = intersectionNum;
+					sendBuffer[3] = getIntersectPos(sign);
+					sendBuffer[4] = turnDir;
+					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
+					orb2_set_color(ORANGE);
+					rtc_reset();
+				}
+			}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0 && prevBot!=packet[1]){
+				sendBuffer[0] = WINTERSECTIONREPLY;
+				sendBuffer[2] = intersectionNum;
+				sendBuffer[3] = packet[1];
+				wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
+				nextBot = packet[1];
+				nextDir = packet[3];
+				nextPath = packet[4];
+				orb2_set_color(BLUE);
+				//delay_ms(200);
+			}
+				delay_ms(0);
+		}
+	}
+}
+
+void waitInIntersectionQueue(){
+	while(prevBot != 0){
+		dataLength = 0;
+		packet = NULL;
+		packet = wl_basic_do_default(&dataLength);
+		if(packet && dataLength==PACKET_LENGTH){
+			if(packet[2] == intersectionNum){
+				if(packet[1]==prevBot && (packet[0]==WINTERSECTIONGO || packet[0]==WINTERSECTIONEXIT)){
+					prevBot = 0;
+					orb2_set_color(PURPLE);
+				}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
+					sendBuffer[0] = WINTERSECTIONREPLY;
+					sendBuffer[2] = intersectionNum;
+					sendBuffer[3] = packet[1];
+					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
+					nextBot = packet[1];
+					nextDir = packet[3];
+					nextPath = packet[4];
+					orb2_set_color(BLUE);
+				}
+			}
+/*
+			if(ISPING(packet)){
+				sendBuffer[0] = WPINGREPLY;
+				sendBuffer[2] = packet[1];
+				if(packet[0]==WPINGQUEUE && packet[3]==nextBot){
+					nextBot = packet[1];
+				}
+				wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
+			}
+			if(packet[0]==WPINGREPLY && packet[2]==id){
+				prevBot = packet[1];
+				pingWaitTime = rtc_get();
+			}
+*/
+		}
+					
+		if(prevBot && rtc_get() << 12 == 0){
+			sendBuffer[0] = WPINGBOT;
+			sendBuffer[2] = prevBot;
+			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
+			pingWaitTime = rtc_get();
+		}
+		if(prevBot && pingWaitTime - rtc_get() > 4){
+			sendBuffer[0] = WPINGBOT;
+			if(pingWaitTime - rtc_get() > 8){
+				sendBuffer[0] = WPINGQUEUE;
+			}
+			sendBuffer[2] = prevBot;
+			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
+		}
+/*				if(prevBot && pingWaitTime - rtc_get() > 12){
+			prevBot = 0;
+		}
+*/
+	}
+}
+
+void driveThroughIntersection(int sign){
+	//Code to choose path through intersection goes in this while loop
+	//But here's the code to handle wireless while in the intersection...
+	start();
+	turn(getIntersectType(0), turnDir);
+	while(doDrive(200) != FINISHED){
+		dataLength = 0;
+		packet = NULL;
+		packet = wl_basic_do_default(&dataLength);		
+		if(dataLength==PACKET_LENGTH){
+			if(packet[2]==intersectionNum/*Intersection Num*/){
+				if(packet[0]==WINTERSECTIONEXIT){
+					prevBot = 0;
+					orb2_set_color(RED);
+				}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
+					sendBuffer[0] = WINTERSECTIONREPLY;
+					sendBuffer[2] = intersectionNum;//Intersection #
+					sendBuffer[3] = packet[1];
+					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
+					nextBot = packet[1];
+					nextDir = packet[3];
+					nextPath = packet[4];
+					orb2_set_color(YELLOW);
+				}
+			}
+			if(ISPING(packet)){
+				sendBuffer[0] = WPINGREPLY;
+				sendBuffer[2] = packet[1];
+				if(packet[0]==WPINGQUEUE && packet[3]==nextBot){
+					nextBot = packet[1];
+				}
+				wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
+			}
+		}
+		if(prevBot==0 && nextBot!=0){//let 2 bots go through at same time
+			/*if(bots paths won't collide){
+				sendBuffer[0] = WINTERSECTIONGO;
+				sendBuffer[2] = 0;//Intersection #
+				wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
+			*/
+		}
+	}
+}
+#endif
+#endif
Index: trunk/code/projects/traffic_navigation/mapping.cold
===================================================================
--- trunk/code/projects/traffic_navigation/mapping.cold	(nonexistent)
+++ trunk/code/projects/traffic_navigation/mapping.cold	(revision 1979)
@@ -0,0 +1,151 @@
+#include ""
+#include "mapping.h"
+#include "lineDrive.h"
+#include <dragonfly_lib.h>
+
+//The last intersection that we encountered
+int lastInt;
+
+
+/* 
+ * Traverses the map using DFS 
+ * Returns 0 if all of the intersections in the database were seen
+ * 1 otherwise 
+ */
+int createEdge(edge* newEdge){
+	char barcode;
+	char time;
+	rtc_init(SIXTEENTH_SECOND, NULL);
+	rtc_reset();
+	while(barcode = NOBARCODE){
+		barcode = (char) doDrive(200);
+	}
+	time = rtc_get();
+	newEdge.to = barcode;
+	newEdge.dist = time;
+	return 0;
+}
+
+int createMap(){
+
+    int seenout_count = 0;			/* The number of intersections we have completely seen */
+    //int hist_count = 0;			/* The size of our history stack */ 
+
+    char[NUM_FEATURES] seen;		/* Have we seen this node? */
+    char[NUM_FEATURES] seenout;		/* Have we seen the outoging edges of this node? */
+    //char[NUM_FEATURES] history;		/* A stack representing the history */
+
+    int i;
+    int outEdges, currInt, chosenEdge;
+
+
+    /* Initialize all of these arrays */
+    for(i = 0; i<NUM_FEATURES; i++) {
+            seen[i] = 0;
+            seenout[i] = 0;
+            history[i] = 0;
+            intersections[i].type = 0;
+            intersections[i].intNum = 0;
+            intersections[i].numOut = 0;
+            intersections[i].outSeen = 0;
+            intersections[i].outgoingEdges[0].to = 0;
+            intersections[i].outgoingEdges[0].dist = 0;
+            intersections[i].outgoingEdges[1].to = 0;
+            intersections[i].outgoingEdges[1].dist = 0;
+            intersections[i].outgoingEdges[2].to = 0;
+            intersections[i].outgoingEdges[2].dist = 0;
+            intersections[i].outgoingEdges[3].to = 0;
+            intersections[i].outgoingEdges[3].dist = 0;	
+    }
+
+
+        /* Drives to the nearest intersection */
+        barcode = nextInt();
+
+        while(seenout_count < NUM_FEATURES)
+        {
+                currInt = getIntersectionNum(barcode);
+                seen[currInt] = 1;
+
+                /* Get the number of outgoing edges */
+                outEdges = getNumOut(getIntersectType(currInt));
+
+                /* Randomly choose an outgoing edge that we have not seen to go down 
+                 * if no such edge exists */
+                chosenEdge = rand() % outEdges;
+
+                /* We have not seen all the outgoing edges */
+                if(!seenout[currInt]){
+                        intersection[currInt].outSeen++;
+                        /* We have finished seeing all outgoing edges of the intersection */
+                        if(intersections[currInt].numOut == intersection[currInt].outSeen){
+                                seenout[currInt] = 1;
+                                seenout_count ++;
+                        }
+                }
+                /* Traverses edge, stores information in struct */
+                traverseEdge(chosenEdge, &(intersection[currInt].outgoingEdges[chosenEdge]));
+        }
+
+        /* We are done traversing send the graph to the robots */
+        sendIntersectionGraph();
+
+
+        return 0;
+}
+
+/* Given an intersection type, returns the number of outgoing edges */
+int getNumOut(int type) {
+    switch(type){
+        case 0: return;
+        case 1:	return; 
+        case 2:	return; 
+        case 3:	return;
+        case 4:	return;
+    }
+
+    return -1;
+}
+
+
+/* Creates an edge in the graph */
+int insertEdge(){
+
+}
+
+
+/* 
+ * Drives to the next intersection and returns its ID
+ * If we are at a dead end, returns -1
+ */
+int nextInt(){
+
+}
+
+
+/*
+ * Given an intersection node returns an integer that
+ * can be sent wirelessly
+ *
+ */
+int encodeNode(node n){
+
+
+}
+
+/*
+ * Uses wireless to send the graph sructure
+ * to other robots
+ *
+ * @return 
+ *
+ */
+void sendIntersectionGraph() {
+        int i;
+        node_union graph;
+
+        for(i=0; i< NUM_FEATURES; i++){
+                graph.n = intersctions[i];
+                wl_basic_send_global(42, graph.array, 12);
+        }
+}
