Index: main-new.c
===================================================================
--- main-new.c	(revision 1987)
+++ main-new.c	(revision 1988)
@@ -1,14 +1,30 @@
 /*
  * main.c for Traffic Navigation
- * Runs the highest level behavior for the Dynamic Traffic Navigation (DTM) SURG
+ * Runs the highest level behavior for the Dynamic Traffic Navigation (DTN) SURG
  *
  * Author: Colony Project, CMU Robotics Club
  */
 
+#define MAIN_NEW
+
 #include "traffic_navigation.h"
 #include "../linefollowing/lineDrive.h"
-#ifndef MAIN_NEW
+#ifdef MAIN_NEW
 
+#ifdef ORB_INTERSECTION
+  #define ORB1_DBG_CLR(color) orb1_set_color(color)
+  #define ORB2_DBG_CLR(color) orb2_set_color(color)
+#else
+  #define ORB1_DBG_CLR(color)
+  #define ORB2_DBG_CLR(color)
+#endif
+
+#ifdef DEBUG_INTERSECTION
+  #define DBG_USBS(str) usb_puts(str)
+#else
+  #define DBG_USBS(str)
+#endif
+
 	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;
@@ -33,9 +49,9 @@
 	
 	id = get_robotid();
 	sign = 0;
-	orb1_set_color(GREEN);
+	ORB1_DBG_CLR(GREEN);
 	delay_ms(1000);
-	orb1_set_color(ORB_OFF);
+	ORB1_DBG_CLR(ORB_OFF);
 	
 	//Test code
 	state = SROAD;
@@ -43,7 +59,7 @@
 	sendBuffer[1] = id;
 
 	/*
-	doDrive(200);
+	doDrive(180);
 	turn(DOUBLE, ILEFT);
 	*/
 	/*
@@ -52,20 +68,18 @@
 	*/
 	
 	while (1) {
-		/*DTM Finite State Machine*/
+		/*DTN 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
+      ORB1_DBG_CLR(WHITE);
+      ORB2_DBG_CLR(ORB_OFF);
 			start();
 			done = false;
 			while(!done){
-				sign = doDrive(200);
+				sign = doDrive(180);
 				switch(sign){
 					case NORMAL:
 					case FINISHED:
@@ -84,14 +98,10 @@
 		case SINTERSECTION_ENTER:/*Entering, and in intersection*/
 			stop();
 			doDrive(0);
-#ifdef ORB_INTERSECTION
-			orb1_set_color(RED);
-			orb2_set_color(ORB_OFF);
-#endif
+      ORB1_DBG_CLR(RED);
+      ORB2_DBG_CLR(ORB_OFF);
 
-#ifdef DEBUG_INTERSECTION
-			usb_puts("STATE: SINTERSECTION_ENTER\n");
-#endif
+			DBG_USBS("STATE: SINTERSECTION_ENTER\n");
 
 			/*Intersection queue:
 			 *Each robot when entering the intersection will check for other robots
@@ -124,34 +134,24 @@
 			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
+					ORB2_DBG_CLR(PURPLE);
 					enterIntersection();
 					retried = 1;
 				}
 				switch (ret) {
 					case KPLACEDINQUEUE:
-#ifdef ORB_INTERSECTION
-						orb2_set_color(GREEN);
-#endif
+						ORB2_DBG_CLR(GREEN);
 						done = true;
 						break;
 					case KFAILEDTOQUEUE:
-#ifdef DEBUG_INTERSECTION
-						usb_puts("Failed to queue\n");
-#endif
-#ifdef ORB_INTERSECTION
-						orb2_set_color(RED);
-#endif
+						DBG_USBS("Failed to queue\n");
+						ORB2_DBG_CLR(RED);
 						enterIntersection();
 						rtc_reset();
 						break;
 					case KRESOLVINGENTER:
 
-#ifdef ORB_INTERSECTION
-						orb2_set_color(ORANGE);
-#endif
+						ORB2_DBG_CLR(ORANGE);
 						state = SINTERSECTION_ENTER_RESOLV;
 						done = true;
 						break;
@@ -160,13 +160,9 @@
 			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
+			ORB1_DBG_CLR(PURPLE);
+			ORB2_DBG_CLR(ORB_OFF);
+			DBG_USBS("STATE: SINTERSECTION_ENTER_RESOLV\n");
 
 			rtc_reset();
 			done = false;
@@ -175,14 +171,14 @@
 				int ret = wirelessPacketHandle(SINTERSECTION_ENTER_RESOLV);
 				switch (ret) {
 					case KRESOLVINGENTER:
-#ifdef ORB_INTERSECTION
-						orb2_set_color(YELLOW);
-#endif
+
+						ORB2_DBG_CLR(YELLOW);
+
 						break;
 					case KPLACEDINQUEUE:
-#ifdef ORB_INTERSECTION
-						orb2_set_color(GREEN);
-#endif
+
+						ORB2_DBG_CLR(GREEN);
+
 						done = true;
 						break;
 					case KFAILEDTOQUEUE:
@@ -218,32 +214,31 @@
 			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
 
+			ORB1_DBG_CLR(YELLOW);
+			ORB2_DBG_CLR(ORB_OFF);
+
+			DBG_USBS("STATE: SINTERSECTION_WAIT\n");
+
+
 			while(queuePrevBot != (char) -1){
 				int ret = wirelessPacketHandle(state);
 				switch (ret){
 					case KFIRSTINQUEUE:
-#ifdef ORB_INTERSECTION
-						orb2_set_color(GREEN);
-#endif
+
+						ORB2_DBG_CLR(GREEN);
+
 						state = SINTERSECTION_DRIVE;
 						break;
 					case KREPLIEDTOENTER:
-#ifdef ORB_INTERSECTION
-						orb2_set_color(BLUE);
-#endif
+
+						ORB2_DBG_CLR(BLUE);
+
 						break;
 					case KRESOLVINGENTER:
-#ifdef ORB_INTERSECTION
-						orb2_set_color(ORANGE);
-#endif
+
+						ORB2_DBG_CLR(ORANGE);
+
 						break;
 				}
 			}
@@ -251,37 +246,36 @@
 			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
+			DBG_USBS("STATE: SINTERSECTION_DRIVE\n");
+
+
+			ORB1_DBG_CLR(GREEN);
+			ORB2_DBG_CLR(ORB_OFF);
+
 			start();
 			turn(getIntersectType(sign), turnDir);
-			while(doDrive(200) != FINISHED){
+			while(doDrive(180) != FINISHED){
 			//while(!button2_click()){
 				 int ret = wirelessPacketHandle(state);
 				 switch (ret){
 					 case KREPLIEDTOENTER:
-#ifdef ORB_INTERSECTION
-						orb2_set_color(BLUE);
-#endif
+
+						ORB2_DBG_CLR(BLUE);
+
 						break;
 					 case KRESOLVINGENTER:
-#ifdef ORB_INTERSECTION
-					 	orb2_set_color(ORANGE);
-#endif
+
+					 	ORB2_DBG_CLR(ORANGE);
+
 						break;
 				 }
 			 }
 			
 			//Exits intersection
-#ifdef ORB_INTERSECTION
-			orb1_set_color(WHITE);
-#endif
 
+			ORB1_DBG_CLR(WHITE);
+
+
 			sendBuffer[0] = WINTERSECTIONEXIT;
 			sendBuffer[2] = intersectionNum;//Intersection #
 			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
@@ -303,9 +297,9 @@
 			state = SROAD;
 			break;
 		case SHIGHWAY:/*On highway*/
-#ifdef ORB_INTERSECTION
-			orb1_set_color(CYAN);
-#endif
+
+			ORB1_DBG_CLR(CYAN);
+
 			while(!button1_click()){
 				highwayFSM();
 			}
@@ -517,4 +511,5 @@
 	sendBuffer[4] = turnDir;
 	wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
 }
+
 #endif
