Index: trunk/code/projects/traffic_navigation/main-intersectionTest.c
===================================================================
--- trunk/code/projects/traffic_navigation/main-intersectionTest.c	(nonexistent)
+++ trunk/code/projects/traffic_navigation/main-intersectionTest.c	(revision 1918)
@@ -0,0 +1,266 @@
+/*
+ * 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"
+
+	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 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(14);// Temporary, restore to 13
+	initializeData();
+	
+	id = get_robotid();
+	sign = 0;
+	
+	//Test code
+	state = SINTERSECTION;
+
+	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);
+			*/
+		}
+	}
+}
