Index: trunk/code/projects/traffic_navigation/main.c
===================================================================
--- trunk/code/projects/traffic_navigation/main.c	(revision 1858)
+++ trunk/code/projects/traffic_navigation/main.c	(revision 1859)
@@ -49,14 +49,19 @@
 #define WINTERSECTIONREPLY 11 //[type, fromBot, intersection, toBot]
 #define WINTERSECTIONEXIT 12 //[type, bot, intersection]
 #define WINTERSECTIONGO 13 //[type, bot, intersection]
+#define WINTERSECTIONPOLICEENTRY 14
 #define WHIGHWAYENTRY 20 //[type, bot, highway]
 #define WHIGHWAYREPLY 21 //[type, fromBot, highway, toBot]
 #define WHIGHWAYEXIT 22 //[type, bot, highway]
+#define WPINGGLOBAL 30 //[type, bot]
+#define WPINGBOT 31 //[type, fromBot, toBot]
+#define WPINGQUEUE 32 //[type, bot, queue]
+#define WPINGREPLY 33 //[type, fromBot, toBot]
 
 int main (void) {
 
 	int state, sign, dataLength;
-	char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, *packet, nextDir, nextPath;
+	char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, *packet, nextDir, nextPath, intersectionNum;
 
 	/* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */
 	dragonfly_init(ALL_ON);
@@ -64,10 +69,15 @@
 	encoders_init();
 	lineFollow_init();
 	rtc_init(SIXTEENTH_SECOND, NULL);	
-
+	wl_basic_init_default();
+	wl_set_channel(13);
+	
 	id = get_robotid();
 	sign = 0;
 	
+	//Test code
+	state = SROAD;
+
 	sendBuffer[1] = id;
 
 	while (1) {
@@ -74,11 +84,11 @@
 		/*DTM Finite State Machine*/
 		switch(state){
 		case SROAD:/*Following a normal road*/
-			sign = lineFollow();
+//			sign = lineFollow();
 			//other road behaviors
 				//tailgating?
 			//read barcode
-			if(sign & CINTERSECTION){
+			if((sign & CINTERSECTION) || button1_click()){
 				state = SINTERSECTION;
 			}
 			break;
@@ -88,6 +98,8 @@
 				//no-stop?
 			//check wireless
 			
+			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.
@@ -96,16 +108,18 @@
 			nextBot = 0;
 			//Sends packet announcing its entry to the intersection
 			sendBuffer[0] = WINTERSECTIONENTRY;
-			sendBuffer[2] = 0;//Intersection #
+			sendBuffer[2] = intersectionNum;//Intersection #
 			sendBuffer[3] = 0;//From Direction
-			sendBuffer[4] = 0;//To Direction
+			sendBuffer[4] = 2;//To Direction
 			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
+			orb1_set_color(BLUE);
 			rtc_reset();
 			while(rtc_get() < 8){//waits for a reply, otherwise assumes it is first in queue
 				packet = wl_basic_do_default(&dataLength);
-				if(dataLength==PACKET_LENGTH && packet[0]==WINTERSECTIONREPLY && packet[2]==0/*Intersection Num*/){
+				if(dataLength==PACKET_LENGTH && packet[0]==WINTERSECTIONREPLY && packet[2]==intersectionNum/*Intersection Num*/){
 					if(packet[3]==id){//Reply for me
 						prevBot = packet[1];
+						orb2_set_color(GREEN);
 						break;
 					}else{//Someone else got here first, try again
 						wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
@@ -113,39 +127,45 @@
 					}
 				}
 			}
+			orb1_set_color(PURPLE);
 			//waits for its turn
 			while(prevBot != 0){
 				packet = wl_basic_do_default(&dataLength);		
-				if(dataLength==PACKET_LENGTH && packet[2]==0/*Intersection Num*/){
+				if(dataLength==PACKET_LENGTH && packet[2]==intersectionNum/*Intersection Num*/){
 					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] = 0;//Intersection #
+						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(BLUE);
 					}
 				}
 			}
+			orb1_set_color(RED);
 			//Drives through intersection
 			//Code to choose path through intersection goes in this while loop
 			//But here's the code to handle wireless while in the intersection...
-			while(1/*replace with variable to keep track of if in intersection*/){
+			while(!button1_click()/*replace with variable to keep track of if in intersection*/){
 				packet = wl_basic_do_default(&dataLength);		
-				if(dataLength==PACKET_LENGTH && packet[2]==0/*Intersection Num*/){
+				if(dataLength==PACKET_LENGTH && 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] = 0;//Intersection #
+						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(prevBot==0 && nextBot!=0){//let 2 bots go through at same time
@@ -158,8 +178,9 @@
 			}
 			//Exits intersection
 			sendBuffer[0] = WINTERSECTIONEXIT;
-			sendBuffer[2] = 0;//Intersection #
+			sendBuffer[2] = intersectionNum;//Intersection #
 			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
+			orb1_set_color(ORANGE);
 			
 			if(!(sign & CINTERSECTION)){
 				if(sign & CHIGHWAYROAD){
@@ -170,7 +191,7 @@
 			}
 			break;
 		case SHIGHWAY:/*On highway*/
-			sign = lineFollow();
+//			sign = lineFollow();
 			//highway behaviors
 				//merging
 				//passing
