Index: branches/wireless/code/projects/unit_tests/test_wireless2.c
===================================================================
--- branches/wireless/code/projects/unit_tests/test_wireless2.c	(revision 1657)
+++ branches/wireless/code/projects/unit_tests/test_wireless2.c	(revision 1658)
@@ -1,12 +1,11 @@
-#include <stdlib.h>
-#include <stdio.h>
+#include <wl_defs.h>
 #include <string.h>
 #include <wireless.h>
 #include <lights.h>
 #include <dragonfly_lib.h>
 #include <xbee.h>
-#include <avr/pgmspace.h>
 
+
 void group_1_function(uint8_t* data,uint8_t length,uint8_t source);
 void group_2_function(uint8_t* data,uint8_t length,uint8_t source);
 void group_3_function(uint8_t* data,uint8_t length,uint8_t source);
@@ -26,29 +25,29 @@
 	usb_init();
 	int status = 0;
 
-	usb_puts("New Wireless Library Test Suite\r\n");
+	WL_DEBUG_PRINT_P("New Wireless Library Test Suite\r\n");
 		
 	/*Initialize the wirelss library*/
-	usb_puts("Initializing library...\r\n");
+	WL_DEBUG_PRINT_P("Initializing library...\r\n");
 	orb1_set_color(BLUE);
 	status = wl_init();
 	switch(status){
 		case 0:
-			usb_puts("Library initialization successful\r\n\r\n");
+			WL_DEBUG_PRINT_P("Library initialization successful\r\n\r\n");
 			orb1_set_color(GREEN);
 			break;
 		case -1:
-			usb_puts("Initialization failed: already initialized\r\n\r\n");
+			WL_DEBUG_PRINT_P("Initialization failed: already initialized\r\n\r\n");
 			orb1_set_color(GREEN);
 			break;
 		case -2:
-			usb_puts("Initialization failed: XBEE initialization failed\r\n\r\n");
+			WL_DEBUG_PRINT_P("Initialization failed: XBEE initialization failed\r\n\r\n");
 			orb1_set_color(RED);
 			break;
 		default:
-			usb_puts("Error: Unreconnized status code: ");
+			WL_DEBUG_PRINT_P("Error: Unreconnized status code: ");
 			usb_puti(status);
-			usb_puts("\r\n\r\n");
+			WL_DEBUG_PRINT_P("\r\n\r\n");
 			orb1_set_color(RED);
 			break;
 	}
@@ -56,58 +55,58 @@
 
 	/*Test all constants defined correctly*/
 	status = 0;
-	usb_puts("Testing constants...");
+	WL_DEBUG_PRINT_P("Testing constants...");
 	orb2_set_color(GREEN);
 	if(GLOBAL!=0){
-		usb_puts("\r\nGLOBAL defined as ");
+		WL_DEBUG_PRINT_P("\r\nGLOBAL defined as ");
 		usb_puti(GLOBAL);
 		status++;
 	}
-	usb_puts(".");
+	WL_DEBUG_PRINT_P(".");
 	if(PAN!=1){
-		usb_puts("\r\nPAN defined as ");
+		WL_DEBUG_PRINT_P("\r\nPAN defined as ");
 		usb_puti(PAN);
 		status++;
 	}
-	usb_puts(".");
+	WL_DEBUG_PRINT_P(".");
 	if(BROADCAST!=0xFFFF){
-		usb_puts("\r\nBROADCAST defined as ");
+		WL_DEBUG_PRINT_P("\r\nBROADCAST defined as ");
 		usb_puti(BROADCAST);
 		status++;
 	}
-	usb_puts(".");
+	WL_DEBUG_PRINT_P(".");
 	if(RELIABLE!=0){
-		usb_puts("\r\nRELIABLE defined as ");
+		WL_DEBUG_PRINT_P("\r\nRELIABLE defined as ");
 		usb_puti(RELIABLE);
 		status++;
 	}
-	usb_puts(".");
+	WL_DEBUG_PRINT_P(".");
 	if(FAST!=1){
-		usb_puts("\r\nFAST defined as ");
+		WL_DEBUG_PRINT_P("\r\nFAST defined as ");
 		usb_puti(FAST);
 		status++;
 	}
-	usb_puts(".");
+	WL_DEBUG_PRINT_P(".");
 	if(NORMAL_PRIORITY!=0){
-		usb_puts("\r\nNORMAL_PRIORITY defined as ");
+		WL_DEBUG_PRINT_P("\r\nNORMAL_PRIORITY defined as ");
 		usb_puti(NORMAL_PRIORITY);
 		status++;
 	}
-	usb_puts(".");
+	WL_DEBUG_PRINT_P(".");
 	if(HIGH_PRIORITY!=1){
-		usb_puts("\r\nHIGH_PRIORITY defined as ");
+		WL_DEBUG_PRINT_P("\r\nHIGH_PRIORITY defined as ");
 		usb_puti(HIGH_PRIORITY);
 		status++;
 	}
-	usb_puts(".\r\nConstant test complete. There were ");
+	WL_DEBUG_PRINT_P(".\r\nConstant test complete. There were ");
 	usb_puti(status);
-	usb_puts(" errors\r\n");
+	WL_DEBUG_PRINT_P(" errors\r\n");
 	if(status!=0){
 		orb2_set_color(ORANGE);
 		delay_ms(500);
 	}
 	/*Tests function registration*/
-	usb_puts("\r\nTesting function registration");
+	WL_DEBUG_PRINT_P("\r\nTesting function registration");
 	int registers[8], count;
 	status = 0;
 	orb2_set_color(GREEN);
@@ -121,11 +120,11 @@
 	registers[7] = wl_register_handler(-1, group_4_function, 0);
 	for(count=0; count<=3; count++){
 		if(registers[count] != 0){
-			usb_puts("\r\nFunction registration error: attempt ");
+			WL_DEBUG_PRINT_P("\r\nFunction registration error: attempt ");
 			usb_puti(count);
-			usb_puts(" returned ");
+			WL_DEBUG_PRINT_P(" returned ");
 			usb_puti(registers[count]);
-			usb_puts(" instead of success\r\n");
+			WL_DEBUG_PRINT_P(" instead of success\r\n");
 			orb2_set_color(ORANGE);
 			status++;
 		}
@@ -132,39 +131,39 @@
 	}
 	for(count=4; count<=6; count++){
 		if(registers[count] == 0){
-			usb_puts("\r\nFunction registration error: attempt ");
+			WL_DEBUG_PRINT_P("\r\nFunction registration error: attempt ");
 			usb_puti(count);
-			usb_puts(" succeeded instead of failing\r\n");
+			WL_DEBUG_PRINT_P(" succeeded instead of failing\r\n");
 			orb2_set_color(ORANGE);
 			status++;
 		}
 		if(registers[count] != 0 && registers[count] != -5){
-			usb_puts("\r\nFunction registration error: attempt ");
+			WL_DEBUG_PRINT_P("\r\nFunction registration error: attempt ");
 			usb_puti(count);
-			usb_puts(" returned ");
+			WL_DEBUG_PRINT_P(" returned ");
 			usb_puti(registers[count]);
-			usb_puts(" instead of -5\r\n");
+			WL_DEBUG_PRINT_P(" instead of -5\r\n");
 			orb2_set_color(ORANGE);
 			status++;
 		}
 	}
 	if(registers[7] == 0){
-		usb_puts("\r\nCongrats, you let an array write to index -1\r\n");
+		WL_DEBUG_PRINT_P("\r\nCongrats, you let an array write to index -1\r\n");
 		orb2_set_color(RED);
 		status++;
 	}
-	usb_puts("Registration test completed. There were ");
+	WL_DEBUG_PRINT_P("Registration test completed. There were ");
 	usb_puti(status);
-	usb_puts(" errors\r\n");
+	WL_DEBUG_PRINT_P(" errors\r\n");
 	delay_ms(500);
 	
 	/*Tests XBee functions*/
-	usb_puts("\r\nTesting XBee fuctions...\r\n");
+	WL_DEBUG_PRINT_P("\r\nTesting XBee fuctions...\r\n");
 	status = 0;
 	orb2_set_color(GREEN);
 	unsigned int pan = xbee_get_pan();
 	if(pan != 0){
-		usb_puts("Pan error: defaulted to non-default Pan id :");
+		WL_DEBUG_PRINT_P("Pan error: defaulted to non-default Pan id :");
 		usb_puti(pan);
 		orb2_set_color(ORANGE);
 		status++;
@@ -171,7 +170,7 @@
 	}
 	pan = xbee_set_pan(1);
 	if(pan != 0){
-		usb_puts("\r\nPan error: error setting Pan id: ");
+		WL_DEBUG_PRINT_P("\r\nPan error: error setting Pan id: ");
 		usb_puti(pan);
 		orb2_set_color(ORANGE);
 		status++;
@@ -178,45 +177,45 @@
 	}
 	pan = xbee_get_pan();
 	if(pan != 0){
-		usb_puts("\r\nPan error: Pan id reads different than set value 1: ");
+		WL_DEBUG_PRINT_P("\r\nPan error: Pan id reads different than set value 1: ");
 		usb_puti(pan);
 		orb2_set_color(ORANGE);
 		status++;
 	}
 	
-	usb_puts("\r\nXBee tests completed. There were ");
+	WL_DEBUG_PRINT_P("\r\nXBee tests completed. There were ");
 	usb_puti(status);
-	usb_puts(" errors.\r\n");
+	WL_DEBUG_PRINT_P(" errors.\r\n");
 
 	/*Tests sending in fast mode*/
-	usb_puts("\r\nTests sending basic packets in fast mode...\r\n");
+	WL_DEBUG_PRINT_P("\r\nTests sending basic packets in fast mode...\r\n");
 	status = 0;
 	uint16_t data = xbee_get_address();
 	status = wl_send((uint8_t*)&data, 2, 0, GLOBAL, BROADCAST, FAST);
-	usb_puts("Global fast broadcast basic send exit code ");
+	WL_DEBUG_PRINT_P("Global fast broadcast basic send exit code ");
 	usb_puti(status);
 	status = wl_send((uint8_t*)&data, 2, 0, GLOBAL, PAN, FAST);
-	usb_puts("\r\nGlobal fast pan send basic exit code ");
+	WL_DEBUG_PRINT_P("\r\nGlobal fast pan send basic exit code ");
 	usb_puti(status);
 	status = wl_send((uint8_t*)&data, 2, 0, GLOBAL, BROADCAST, FAST);
-	usb_puts("\r\nGlobal fast broadcast basic send exit code ");
+	WL_DEBUG_PRINT_P("\r\nGlobal fast broadcast basic send exit code ");
 	usb_puti(status);
 	status = wl_send((uint8_t*)&data, 2, 0, GLOBAL, PAN, FAST);
-	usb_puts("\r\nGlobal fast pan send basic exit code ");
+	WL_DEBUG_PRINT_P("\r\nGlobal fast pan send basic exit code ");
 	usb_puti(status);
 	status = wl_send((uint8_t*)&data, 2, 1, GLOBAL, BROADCAST, FAST);
-	usb_puts("\r\nGlobal fast broadcast group 1 send exit code ");
+	WL_DEBUG_PRINT_P("\r\nGlobal fast broadcast group 1 send exit code ");
 	usb_puti(status);
 	status = wl_send((uint8_t*)&data, 2, 2, GLOBAL, PAN, FAST);
-	usb_puts("\r\nGlobal fast pan send group 2 exit code ");
+	WL_DEBUG_PRINT_P("\r\nGlobal fast pan send group 2 exit code ");
 	usb_puti(status);
 	status = wl_send((uint8_t*)&data, 2, 1, GLOBAL, BROADCAST, FAST);
-	usb_puts("\r\nGlobal fast broadcast group 3 send exit code ");
+	WL_DEBUG_PRINT_P("\r\nGlobal fast broadcast group 3 send exit code ");
 	usb_puti(status);
 	status = wl_send((uint8_t*)&data, 2, 2, GLOBAL, PAN, FAST);
-	usb_puts("\r\nGlobal fast pan send group 4 exit code ");
+	WL_DEBUG_PRINT_P("\r\nGlobal fast pan send group 4 exit code ");
 	usb_puti(status);
-	usb_puts("\r\nFast send tests successful\r\n");
+	WL_DEBUG_PRINT_P("\r\nFast send tests successful\r\n");
 
 	/*Sends packets in fast mode until other robot responds*/
 	status = 0;
@@ -228,7 +227,7 @@
 		wl_get_basic(address, 2);
 		if(address != 0){
 			orb2_set_color(GREEN);
-			usb_puts("\r\nFriend address: ");
+			WL_DEBUG_PRINT_P("\r\nFriend address: ");
 			usb_puti((uint16_t)address);
 			friendAddress = *address;
 		}
@@ -244,33 +243,33 @@
 	
 
 	/*Terminates wireless functions*/
-	usb_puts("\r\n\r\nTerminating wireless...\r\n");
+	WL_DEBUG_PRINT_P("\r\n\r\nTerminating wireless...\r\n");
 	status = wl_init();
 	switch(status){
 		case 0:
-			usb_puts("Wireless termination successful\r\n\r\n");
+			WL_DEBUG_PRINT_P("Wireless termination successful\r\n\r\n");
 			orb1_set_color(BLUE);
 			break;
 		case -3:
-			usb_puts("Termination failed: library not initialized\r\n\r\n");
+			WL_DEBUG_PRINT_P("Termination failed: library not initialized\r\n\r\n");
 			orb1_set_color(BLUE);
 			break;
 		case -5:
-			usb_puts("Termination failed\r\n\r\n");
+			WL_DEBUG_PRINT_P("Termination failed\r\n\r\n");
 			orb1_set_color(RED);
 			break;
 		case -6:
-			usb_puts("Termination failed: function unregistration failed\r\n\r\n");
+			WL_DEBUG_PRINT_P("Termination failed: function unregistration failed\r\n\r\n");
 			orb1_set_color(ORANGE);
 			break;
 		default:
-			usb_puts("Error: Unreconnized status code: ");
+			WL_DEBUG_PRINT_P("Error: Unreconnized status code: ");
 			usb_puti(status);
 			orb1_set_color(RED);
 			break;
 	}
 
-	usb_puts("\r\n\r\nWireless Library tests completed");
+	WL_DEBUG_PRINT_P("\r\n\r\nWireless Library tests completed");
 	while(1){}
 }
 
@@ -343,7 +342,7 @@
 		usb_puti(wl_send_robot(&count, 1, 4, friendAddress, RELIABLE));
 		delay_ms(count * 10);
 	}
-	usb_puts("ACK Errors: ");
+	WL_DEBUG_PRINT_P("ACK Errors: ");
 	usb_puti(wl_ack_error());
 	wl_ack_reset();
 	usb_puti(wl_ack_error());
@@ -352,7 +351,7 @@
 		usb_puti(wl_send_robot(&count, 1, 0, friendAddress + 1, RELIABLE));
 		delay_ms(count * 10);
 	}
-	usb_puts("ACK Errors: ");
+	WL_DEBUG_PRINT_P("ACK Errors: ");
 	usb_puti(wl_ack_error());
 }
 
@@ -368,19 +367,19 @@
 }
 
 void group_1_function(uint8_t* data,uint8_t length,uint8_t source){
-	usb_puts("\r\nFunction 1 called");
+	WL_DEBUG_PRINT_P("\r\nFunction 1 called");
 	return 0;
 }
 void group_2_function(uint8_t* data,uint8_t length,uint8_t source){
-	usb_puts("\r\nFunction 2 called");
+	WL_DEBUG_PRINT_P("\r\nFunction 2 called");
 	return 0;
 }
 void group_3_function(uint8_t* data,uint8_t length,uint8_t source){
-	usb_puts("\r\nFunction 3 called");
+	WL_DEBUG_PRINT_P("\r\nFunction 3 called");
 	return 0;
 }
 void group_4_function(uint8_t* data,uint8_t length,uint8_t source){
-	usb_puts("\r\nFunction 4 called");
+	WL_DEBUG_PRINT_P("\r\nFunction 4 called");
 	return 0;
 }
 //The first robot to receive the address of another robot will trigger this function.
Index: branches/wireless/code/projects/unit_tests/test_xbee.c
===================================================================
--- branches/wireless/code/projects/unit_tests/test_xbee.c	(revision 1657)
+++ branches/wireless/code/projects/unit_tests/test_xbee.c	(revision 1658)
@@ -31,7 +31,9 @@
   }
   
 	usb_puts("\r\n\n");
-	delay_ms(1000);
+  while(1) {
+    delay_ms(10000);
+  }
   
   // check registers
   WL_DEBUG_PRINT("receive interrupt flag(80)=");
@@ -92,7 +94,7 @@
   } else {
     usb_puts("error getting pan\r\n");
   }
-  
+    
   // end of tests
   if (xbee_terminate() != 0) {
     usb_puts("xbee_terminate error\r\n");
Index: branches/wireless/code/projects/libwireless/wl_defs.h
===================================================================
--- branches/wireless/code/projects/libwireless/wl_defs.h	(revision 1657)
+++ branches/wireless/code/projects/libwireless/wl_defs.h	(revision 1658)
@@ -145,3 +145,5 @@
 #define WL_DEBUG_PRINT_HEX( i )
 
 #endif
+
+#endif
\ No newline at end of file
Index: branches/wireless/code/projects/libdragonfly/serial.c
===================================================================
--- branches/wireless/code/projects/libdragonfly/serial.c	(revision 1657)
+++ branches/wireless/code/projects/libdragonfly/serial.c	(revision 1658)
@@ -141,14 +141,15 @@
  * @return 0 if init succesfull, an error code otherwise
  **/
 int usb_puts_P (PGM_P s) {
-    char buf;
-
     if(!usb_initd)
       return ERROR_LIBRARY_NOT_INITD;
-	
-    while (memcpy_P (&buf, s, sizeof (char)), buf!=0) {
-        usb_putc (buf);
-        s++;
+      
+    while (pgm_read_byte(s) != 0x00) {
+        // Wait until buffer is clear for sending
+        loop_until_bit_is_set(UCSR0A, UDRE0);
+        
+        // Load buffer with your character
+        UDR0 = pgm_read_byte(s++);
     }
 
     return 0;
