Index: xbee.c
===================================================================
--- xbee.c	(revision 667)
+++ xbee.c	(revision 668)
@@ -1,6 +1,6 @@
 /**
  * Copyright (c) 2007 Colony Project
- * 
+ *
  * Permission is hereby granted, free of charge, to any person
  * obtaining a copy of this software and associated documentation
  * files (the "Software"), to deal in the Software without
@@ -9,10 +9,10 @@
  * copies of the Software, and to permit persons to whom the
  * Software is furnished to do so, subject to the following
  * conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be
  * included in all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
  * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
@@ -55,6 +55,7 @@
 #include <string.h>
 
 #define XBEE_FRAME_START 0x7E
+#define XBEE_GET_PACKET_TIMEOUT 1000
 
 /*Frame Types*/
 #define XBEE_FRAME_STATUS 0x8A
@@ -69,41 +70,40 @@
 /*Internal Function Prototypes*/
 
 /*I/O Functions*/
-void xbee_send(char* buf, int size);
-void xbee_send_string(char* c);
+static int xbee_send(char* buf, int size);
+static int xbee_send_string(char* c);
 
 #ifndef ROBOT
-void xbee_read(char* buf, int size);
+static int xbee_read(char* buf, int size);
 #endif
 
 /*Command Mode Functions
  * Called during initialization.
  */
-void xbee_enter_command_mode(void);
-void xbee_exit_command_mode(void);
-void xbee_enter_api_mode(void);
-void xbee_exit_api_mode(void);
-void xbee_wait_for_string(char* s, int len);
-void xbee_wait_for_ok(void);
+static int xbee_enter_command_mode(void);
+static int xbee_exit_command_mode(void);
+static int xbee_enter_api_mode(void);
+static int xbee_exit_api_mode(void);
+static int xbee_wait_for_string(char* s, int len);
+static int xbee_wait_for_ok(void);
 
 /*API Mode Functions*/
 
-int xbee_handle_packet(char* packet, int len);
-void xbee_handle_at_command_response(char* command, char result,
-	char* extra, int extraLen);
-void xbee_handle_status(char status);
-int xbee_verify_checksum(char* packet, int len);
-char xbee_compute_checksum(char* packet, int len);
-void xbee_send_frame(char* buf, int len);
-void xbee_send_read_at_command(char* command);
-void xbee_send_modify_at_command(char* command, char* value);
+static int xbee_handle_packet(char* packet, int len);
+static void xbee_handle_at_command_response(char* command, char result, char* extra, int extraLen);
+static void xbee_handle_status(char status);
+static int xbee_verify_checksum(char* packet, int len);
+static char xbee_compute_checksum(char* packet, int len);
+static int xbee_send_frame(char* buf, int len);
+static int xbee_send_read_at_command(char* command);
+static int xbee_send_modify_at_command(char* command, char* value);
 
 /*Global Variables*/
 
 #ifndef ROBOT
-char* xbee_com_port = XBEE_PORT_DEFAULT;
-int xbee_stream;
-pthread_t* xbee_listen_thread;
+static char* xbee_com_port = XBEE_PORT_DEFAULT;
+static int xbee_stream;
+static pthread_t* xbee_listen_thread;
 #endif
 
 // TODO: is this a good size?
@@ -117,16 +117,25 @@
 
 
 //used to store packets as they are read
-char xbee_buf[128];
-int currentBufPos = 0;
+static char xbee_buf[128];
+static int currentBufPos = 0;
 
 //XBee status
-unsigned int xbee_panID = XBEE_PAN_DEFAULT;
-unsigned int xbee_pending_panID = XBEE_PAN_DEFAULT;
-int xbee_channel = XBEE_CHANNEL_DEFAULT;
-int xbee_pending_channel = XBEE_CHANNEL_DEFAULT;
-unsigned int xbee_address = 0;
+static unsigned int xbee_panID = XBEE_PAN_DEFAULT;
+static unsigned int xbee_pending_panID = XBEE_PAN_DEFAULT;
+static int xbee_channel = XBEE_CHANNEL_DEFAULT;
+static int xbee_pending_channel = XBEE_CHANNEL_DEFAULT;
+static volatile unsigned int xbee_address = 0;
 
+void printHex(char * s, int len) {
+  int i;
+  for (i = 0; i < len; i++) {
+    printf("0x%x ", (int)(s[i]));
+  }
+  printf("\n");
+}
+
+
 /*Function Implementations*/
 
 #ifdef ROBOT
@@ -135,10 +144,10 @@
  * Interrupt for the robot. Adds bytes received from the xbee
  * to the buffer.
  **/
-#ifndef FIREFLY
-ISR(USART1_RX_vect)
+#ifdef FIREFLY
+SIGNAL(SIG_USART0_RECV)
 {
-	char c = UDR1;
+	char c = UDR0;
 	arrival_buf[buffer_last] = c;
 	int t = buffer_last + 1;
 	if (t == XBEE_BUFFER_SIZE)
@@ -150,9 +159,12 @@
 	buffer_last = t;
 }
 #else
-SIGNAL(SIG_USART0_RECV)
+#ifdef BAYBOARD
+// TODO: interrupt handler for bayboard, should be similar to above and below
+#else
+ISR(USART1_RX_vect)
 {
-	char c = UDR0;
+	char c = UDR1;
 	arrival_buf[buffer_last] = c;
 	int t = buffer_last + 1;
 	if (t == XBEE_BUFFER_SIZE)
@@ -164,18 +176,27 @@
 	buffer_last = t;
 }
 #endif
+#endif
 
 #else
 
+// Computer code
+
 /**
  * Thread that listens to the xbee.
  **/
-void* listen_to_xbee(void* x)
+static void* listen_to_xbee(void* x)
 {
 	char c;
 	while (1)
 	{
-		xbee_read(&c, 1);
+		if (xbee_read(&c, 1) != 0) {
+			fprintf(stderr, "xbee_read failed.\n");
+			return NULL;
+		}
+		
+		//DEBUGGING PRINT
+		//printf("interrupt: %c (%d)\n", c, (int)c);
 		arrival_buf[buffer_last] = c;
 		int t = buffer_last + 1;
 		if (t == XBEE_BUFFER_SIZE)
@@ -185,8 +206,11 @@
 			WL_DEBUG_PRINT("Out of space in buffer.\n");
 		}
 		buffer_last = t;
+
+		usleep(1000);
 	}
-	return 0;
+
+	return NULL;
 }
 
 #endif
@@ -194,21 +218,25 @@
 /**
  * Initializes the XBee library so that other functions may be used.
  **/
-int xbee_lib_init(void)
+int xbee_lib_init()
 {
 	arrival_buf[0] = 'A';
 	arrival_buf[1] = 'A';
 	arrival_buf[2] = 'A';
-	#ifdef ROBOT
+#ifdef ROBOT
 
 	//enable the receiving interrupt
-	#ifdef FIREFLY
+#ifdef FIREFLY
 	UCSR0B |= _BV(RXCIE) | _BV(RXEN);
-	#else
+#else
+#ifdef BAYBOARD
+//TODO: enable the receiving interrupt on the bayboard
+#else
 	UCSR1B |= _BV(RXCIE);
-	#endif
+#endif
+#endif
 	sei();
-	#else
+#else
 	printf("Connecting to port %s.\n", xbee_com_port);
 	xbee_stream = open(xbee_com_port, O_RDWR);
 	if (xbee_stream == -1/* || lockf(xbee_stream, F_TEST, 0) != 0*/)
@@ -215,8 +243,10 @@
 	{
 		printf("Failed to open connection to XBee on port %s\r\n", xbee_com_port);
 		return -1;
+	} else {
+	  printf("Successfully opened connection to XBee on port %s\r\n", xbee_com_port);
 	}
-	
+
 	// set baud rate, etc. correctly
 	struct termios options;
 
@@ -238,37 +268,102 @@
 	{
 		fprintf(stderr, "Error setting attributes.\n");
 		return -1;
+	} else {
+	  //printf("Successfully set termios attributes.\n");
 	}
 
 	//lockf(xbee_stream, F_LOCK, 0);
-	
-	xbee_listen_thread =
-		(pthread_t*)malloc(sizeof(pthread_t));
+
+	xbee_listen_thread = (pthread_t*)malloc(sizeof(pthread_t));
 	if (xbee_listen_thread == NULL)
 	{
 		fprintf(stderr, "%s: Malloc failed.\n", __FUNCTION__);
 		return -1;
 	}
-	
-	int ret = pthread_create(xbee_listen_thread, NULL,
-		listen_to_xbee, NULL);
+
+	int ret = pthread_create(xbee_listen_thread, NULL, listen_to_xbee, NULL);
 	if (ret)
 	{
 		fprintf(stderr, "Failed to create listener thread.\r\n");
 		return -1;
+	} else {
+	  //printf("Successfully created listener thread.\n");
 	}
-	
-	#endif
-	xbee_enter_command_mode();
-	xbee_enter_api_mode();
-	xbee_exit_command_mode();
-	xbee_send_read_at_command("MY");
-	
+#endif
+
+	//DEBUGGING PRINT
+	//printf("about to call xbee_enter_command_mode\n");
+
+	if (xbee_enter_command_mode() != 0) {
+#ifndef ROBOT
+	  printf("Error returned from xbee_enter_command_mode\n");
+#endif
+		return -1;
+	}
+
+	//DEBUGGING PRINT
+	//printf("about to call xbee_enter_api_mode\n");
+
+	if (xbee_enter_api_mode() != 0) {
+#ifndef ROBOT
+	  printf("Error returned from xbee_enter_api_mode\n");
+#endif
+		return -1;
+	}
+
+	//DEBUGGING PRINT
+	//printf("about to call xbee_exit_command_mode\n");
+
+	if (xbee_exit_command_mode() != 0) {
+#ifndef ROBOT
+	  printf("Error returned from xbee_exit_command_mode\n");
+#endif
+		return -1;
+	}
+
+	//DEBUGGING PRINT
+	//printf("about to call xbee_send_read_at_command\n");
+
+	if (xbee_send_read_at_command("MY")) {
+#ifndef ROBOT
+	  printf("Error returned from xbee_send_read_at_command\n");
+#endif
+		return -1;
+	}
+
+#ifndef ROBOT
+	//printf("About to enter while loop to get xbee_address.\n");
+	int i;
+	for (i = 0; xbee_address == 0 && i < XBEE_GET_PACKET_TIMEOUT; i++) {
+	  ret = xbee_get_packet(NULL);
+
+	  usleep(1000);
+
+	  if (ret == -1) {
+		//printf("xbee_get_packet(NULL) failed.\n");
+		return -1;
+	  }
+	}
+
+	//	printf("After exiting while loop to get xbee_address.\n");
+#else
 	//wait to return until the address is set
-	while (xbee_address == 0) xbee_get_packet(NULL);
+	while (xbee_address == 0) {
+	  xbee_get_packet(NULL);
+	}
+#endif
 
+#ifndef ROBOT
+	if (i == XBEE_GET_PACKET_TIMEOUT) { // We timed-out.
 
+	  printf("xbee_get_packet timed out.\n");
+	  return -1;
+	} else {
+	  return 0;
+	}
+#else
 	return 0;
+#endif
 }
 
 /**
@@ -287,21 +382,29 @@
 
 /**
  * Send a buffer buf of size bytes to the XBee.
- * 
+ *
  * @param buf the buffer of data to send
  * @param size the number of bytes to send
  **/
-void xbee_send(char* buf, int size)
+static int xbee_send(char* buf, int size)
 {
-	#ifdef ROBOT
+#ifdef ROBOT
 	int i;
-	for (i = 0; i < size; i++)
+	for (i = 0; i < size; i++) {
 		xbee_putc(buf[i]);
-	#else
+	}
+
+	return 0;
+
+#else
+	//DEBUGGING PRINT
+	//printf("in xbee_send ");
+	//printHex(buf, size);
+
 	int ret = write(xbee_stream, buf, size);
 	//success
 	if (ret == size)
-		return;
+		return 0;
 	if (ret == -1)
 	{
 		//interrupted by system signal, probably timer interrupt.
@@ -308,16 +411,15 @@
 		//just try again
 		if (errno == 4)
 		{
-			xbee_send(buf, size);
-			return;
+			return xbee_send(buf, size);
 		}
 		printf("Failed to write to xbee, error %i.\r\n", errno);
-		return;
+		return -1;
 	}
 
 	//write was interrupted after writing ret bytes
-	xbee_send(buf + ret, size - ret);
-	#endif
+	return xbee_send(buf + ret, size - ret);
+#endif
 }
 
 /**
@@ -325,16 +427,20 @@
  *
  * @param c the string to send to the XBEE
  **/
-void xbee_send_string(char* c)
+static int xbee_send_string(char* c)
 {
-	xbee_send(c, strlen(c));
+	return xbee_send(c, strlen(c));
 }
 
 #ifndef ROBOT
-void xbee_read(char* buf, int size)
+static int xbee_read(char* buf, int size)
 {
-	if (read(xbee_stream, buf, size) == -1)
+	if (read(xbee_stream, buf, size) == -1) {
 		printf("Failed to read from xbee.\r\n");
+		return -1;
+	}
+
+	return 0;
 }
 #endif
 
@@ -341,44 +447,69 @@
 /**
  * Enter into command mode.
  **/
-void xbee_enter_command_mode()
+static int xbee_enter_command_mode()
 {
-	xbee_send_string("+++");
-	xbee_wait_for_ok();
+	if (xbee_send_string("+++") != 0) {
+		return -1;
+	}
+
+#ifndef ROBOT
+	//	printf("In xbee_enter_command_mode about to call xbee_wait_for_ok()\n");
+#endif
+
+	if (xbee_wait_for_ok() != 0) {
+#ifndef ROBOT
+	  printf("xbee_wait_for_ok failed.\n");
+#endif
+	  return -1;
+	} else {
+	  return 0;
+	}
 }
 
 /**
  * Exit from command mode.
  **/
-void xbee_exit_command_mode()
+static int xbee_exit_command_mode()
 {
-	xbee_send_string("ATCN\r");
+	if (xbee_send_string("ATCN\r") != 0) {
+		return -1;
+	}
+
 	xbee_wait_for_ok();
+
+	return 0;
 }
 
 /**
  * Enter API mode.
  **/
-void xbee_enter_api_mode()
+static int xbee_enter_api_mode()
 {
-	xbee_send_string("ATAP 1\r");
+	if (xbee_send_string("ATAP 1\r") != 0) {
+		return -1;
+	}
 	xbee_wait_for_ok();
+
+	return 0;
 }
 
 /**
  * Exit API mode. (warning - does not check for response)
  **/
-void xbee_exit_api_mode()
+static int xbee_exit_api_mode()
 {
-	xbee_send_string("ATAP 0\r");
+	return xbee_send_string("ATAP 0\r");
 }
 
 /**
  * Wait until the string "OK\r" is received from the XBee.
  **/
-void xbee_wait_for_ok()
+static int xbee_wait_for_ok()
 {
-	xbee_wait_for_string("OK\r", 3);
+  //DEBUGGING PRINT
+  //printf("\tin xbee_wait_for_ok\n");
+	return xbee_wait_for_string("OK\r", 3);
 }
 
 /**
@@ -388,22 +519,45 @@
  * @param s the string to receive
  * @param len the length of the string
  **/
-void xbee_wait_for_string(char* s, int len)
+static int xbee_wait_for_string(char* s, int len)
 {
+  //DEBUGGING PRINT
+  //printf("\t in xbee_wait_for_string\n");
+
+#ifndef ROBOT
+  //printf("In xbee_wait_for_string.\n");
+#endif
+
 	char* curr = s;
-	while (curr - s < len)
-	{
+	while (curr - s < len) {
 		// check if buffer is empty
-		if (buffer_last == buffer_first)
-			continue;
-		char c = arrival_buf[buffer_first++];
-		if (buffer_first == XBEE_BUFFER_SIZE)
-			buffer_first = 0;
-		if (c == *curr)
-			curr++;
-		else
-			curr = s;
+		if (buffer_last != buffer_first) {
+			char c = arrival_buf[buffer_first++];
+			if (buffer_first == XBEE_BUFFER_SIZE) {
+				buffer_first = 0;
+			}
+
+			//DEBUGGING PRINT
+			//printf("\t\t c is %c (%d)\n", c, (int)c);
+
+			if (c == *curr) {
+				curr++;
+			} else {
+#ifndef ROBOT
+			  //return -1; // Computer is less forgiving.
+			  curr = s;
+#else
+			  curr = s;
+#endif
+			}
+		} // else buffer is empty.
+
+#ifndef ROBOT
+		usleep(100);
+#endif
 	}
+
+	return 0;
 }
 
 /**
@@ -455,16 +609,29 @@
  * @param len the size in bytes of the packet data
  *
  **/
-void xbee_send_frame(char* buf, int len)
+static int xbee_send_frame(char* buf, int len)
 {
+	//printf("in %s and len is %d\n", __FUNCTION__, len);
+
 	char prefix[3];
 	prefix[0] = XBEE_FRAME_START;
 	prefix[1] = (len & 0xFF00) >> 8;
 	prefix[2] = len & 0xFF;
 	char checksum = xbee_compute_checksum(buf, len);
-	xbee_send(prefix, 3);
-	xbee_send(buf, len);
-	xbee_send(&checksum, 1);
+
+	if (xbee_send(prefix, 3) != 0) {
+		return -1;
+	}
+
+	if (xbee_send(buf, len) != 0) {
+		return -1;
+	}
+
+	if (xbee_send(&checksum, 1) != 0) {
+		return -1;
+	}
+
+	return 0;
 }
 
 /**
@@ -474,9 +641,9 @@
  * use ID to read the PAN ID and MY to return the XBee ID.
  * See the XBee reference guide for a complete listing.
  **/
-void xbee_send_read_at_command(char* command)
+static int xbee_send_read_at_command(char* command)
 {
-	xbee_send_modify_at_command(command, NULL);
+	return xbee_send_modify_at_command(command, NULL);
 }
 
 /**
@@ -486,11 +653,13 @@
  * @param value the value to pass as a parameter
  * (or NULL if there is no parameter)
  **/
-void xbee_send_modify_at_command(char* command, char* value)
+static int xbee_send_modify_at_command(char* command, char* value)
 {
+	//printf("in %s with command %s and value %s\n", __FUNCTION__, command, value);
+
 	char buf[16];
 	int i;
-	
+
 	buf[0] = XBEE_FRAME_AT_COMMAND;
 	buf[1] = 1;
 	buf[2] = command[0];
@@ -502,26 +671,29 @@
 		if (valueLen > 8)
 		{
 			WL_DEBUG_PRINT("AT Command too large.\r\n");
-			return;
+			return -1;
 		}
-		for (i = 0; i < valueLen; i++)
+
+		for (i = 0; i < valueLen; i++) {
 			buf[4 + i] = value[i];
+		}
 	}
-	xbee_send_frame(buf, 4 + valueLen);
+
+	return xbee_send_frame(buf, 4 + valueLen);
 }
 
 /**
  * Send the specified packet.
- * 
+ *
  * @param packet the packet data to send
  * @param len the number of bytes in the packet
- * 
+ *
  * @param dest the ID of the XBee to send the packet to,
  * or XBEE_BROADCAST to send the message to all robots
  * in the PAN.
- * 
+ *
  * @param options a combination of the flags
- * XBEE_OPTIONS_NONE, XBEE_OPTIONS_DISABLE_RESPONSE and 
+ * XBEE_OPTIONS_NONE, XBEE_OPTIONS_DISABLE_RESPONSE and
  * XBEE_OPTIONS_BROADCAST_ALL_PANS
  *
  * @param frame the frame number to associate this packet
@@ -529,8 +701,7 @@
  * the XBee alerts us as to whether or not our message
  * was received.
  **/
-void xbee_send_packet(char* packet, int len, int dest,
-	char options, char frame)
+int xbee_send_packet(char* packet, int len, int dest, char options, char frame)
 {
 	char buf[5];
 	char prefix[3];
@@ -540,7 +711,7 @@
 	if (len > 100)
 	{
 		WL_DEBUG_PRINT("Packet is too large.\r\n");
-		return;
+		return -1;
 	}
 
 	//data for sending request
@@ -560,10 +731,24 @@
 	for (i = 0; i < len; i++)
 		checksum += (unsigned char)packet[i];
 	checksum = 0xFF - checksum;
-	xbee_send(prefix, 3);
-	xbee_send(buf, 5);
-	xbee_send(packet, len);
-	xbee_send((char*)&checksum, 1);
+
+	if (xbee_send(prefix, 3) != 0) {
+		return -1;
+	}
+
+	if (xbee_send(buf, 5) != 0) {
+		return -1;
+	}
+
+	if (xbee_send(packet, len) != 0) {
+		return -1;
+	}
+
+	if (xbee_send((char*)&checksum, 1) != 0) {
+		return -1;
+	}
+
+	return 0;
 }
 
 /**
@@ -578,7 +763,7 @@
  *
  * The first byte of the packet will be either
  * XBEE_TX_STATUS or XBEE_RX to indicated
- * a response to a sent message or a received message, 
+ * a response to a sent message or a received message,
  * respectively.<br><br>
  *
  * For a status response packet:<br>
@@ -585,7 +770,7 @@
  * The first byte will be XBEE_TX_STATUS.<br>
  * The second byte will be the frame number.<br>
  * The third byte will be the result. 0 indicates success,
- * and nonzero indicates that an error ocurred in 
+ * and nonzero indicates that an error ocurred in
  * transmitting the packet.<br><br>
  *
  * For a received packet:<br>
@@ -595,7 +780,7 @@
  * The fourth byte is the signal strength.<br>
  * The fifth byte is 1 if the packet were sent to
  * a specific address, and 2 if it is a broadcast packet.<br><br>
- * 
+ *
  * @param dest set to the packet data
  * @return the length of the packet, or -1 if no packet
  * is available
@@ -610,22 +795,25 @@
 			if (buffer_first == XBEE_BUFFER_SIZE)
 				buffer_first = 0;
 			// check if buffer is empty
-			if (buffer_first == buffer_last)
-				return -1;
+			if (buffer_first == buffer_last) {
+				return 0;
+			}
+		} while (arrival_buf[buffer_first++] != XBEE_FRAME_START);
+
+		if (buffer_first == XBEE_BUFFER_SIZE) {
+			buffer_first = 0;
 		}
-		while (arrival_buf[buffer_first++] != XBEE_FRAME_START);
-		if (buffer_first == XBEE_BUFFER_SIZE)
-			buffer_first = 0;
 		xbee_buf[0] = XBEE_FRAME_START;
 		currentBufPos++;
 	}
 
 	int len = -1;
-	if (currentBufPos >= 3)
+	if (currentBufPos >= 3) {
 		len = (int)xbee_buf[2] + ((int)xbee_buf[1] << 8);
-		
+	}
+
 	while (len == -1 //packet length has not been read yet
-			|| currentBufPos < len + 4)
+		|| currentBufPos < len + 4)
 	{
 		if (currentBufPos == 3)
 		{
@@ -637,16 +825,19 @@
 				return -1;
 			}
 		}
+
 		// check if buffer is empty
-		if (buffer_first == buffer_last)
-			return -1;
+		if (buffer_first == buffer_last) {
+			return 0;
+		}
 		xbee_buf[currentBufPos++] = arrival_buf[buffer_first++];
-		if (buffer_first == XBEE_BUFFER_SIZE)
+		if (buffer_first == XBEE_BUFFER_SIZE) {
 			buffer_first = 0;
+		}
 	}
-	
+
 	currentBufPos = 0;
-	
+
 	if (!xbee_verify_checksum(xbee_buf, len + 4))
 	{
 		WL_DEBUG_PRINT("XBee checksum failed.\r\n");
@@ -654,15 +845,19 @@
 	}
 
 	//we will take care of the packet
-	if (xbee_handle_packet(xbee_buf + 3, len))
-		return -1;
-	
-	if (dest == NULL)
-		return -1;
-	
+	if (xbee_handle_packet(xbee_buf + 3, len) != 0) {
+		return 0;
+	}
+
+	if (dest == NULL) {
+		return 0;
+	}
+
 	int i;
-	for (i = 3; i < len + 3; i++)
+	for (i = 3; i < len + 3; i++) {
 		dest[i - 3] = xbee_buf[i];
+	}
+
 	return len;
 }
 
@@ -706,8 +901,7 @@
  * @param extra the hex value of the requested register
  * @param extraLen the length in bytes of extra
  **/
-void xbee_handle_at_command_response(char* command, char result,
-	char* extra, int extraLen)
+static void xbee_handle_at_command_response(char* command, char result, char* extra, int extraLen)
 {
 	if (result == 1)
 	{
@@ -718,7 +912,7 @@
 	WL_DEBUG_PRINT("AT");
 	WL_DEBUG_PRINT(command);
 	WL_DEBUG_PRINT(" command was successful.\r\n");
-		
+
 	if (command[0] == 'I' && command[1] == 'D')
 	{
 		xbee_panID = xbee_pending_panID;
@@ -736,13 +930,17 @@
 		WL_DEBUG_PRINT(".\r\n");
 		return;
 	}
-	
+
 	if (command[0] == 'M' && command[1] == 'Y' && extraLen != 0)
 	{
+//							printf("reading xbee_address\n");
+
 		xbee_address = 0;
 		int i;
-		for (i = 0; i < extraLen; i++)
+		for (i = 0; i < extraLen; i++) {
 			xbee_address = (xbee_address << 8) + extra[i];
+		}
+//							printf("xbee address is: %d\n", xbee_address);
 
 		WL_DEBUG_PRINT("XBee address is ");
 		WL_DEBUG_PRINT_INT(xbee_address);
@@ -750,6 +948,7 @@
 
 		if (xbee_address == 0)
 		{
+		  printf("XBee 16-bit address must be set using ATMY.\r\n");
 			WL_DEBUG_PRINT("XBee 16-bit address must be set using ATMY.\r\n");
 			exit(0);
 		}
@@ -765,11 +964,15 @@
  *
  * @param packet the packet to handle
  * @param len the length of the packet
- * 
+ *
  * @return 1 if we have handled the packet, 0 otherwise
  */
-int xbee_handle_packet(char* packet, int len)
+static int xbee_handle_packet(char* packet, int len)
 {
+  //DEBUGGING PRINT
+  //printf("xbee_handle_packet: ");
+  //printHex(packet, len);
+
 	char command[3] = {1, 2, 3};
 	if (len <= 0) //this should not happend
 	{
@@ -776,7 +979,7 @@
 		WL_DEBUG_PRINT("Non-positive packet length.\r\n");
 		return 0;
 	}
-	
+
 	switch ((unsigned char)packet[0]) //packet type
 	{
 		case XBEE_FRAME_STATUS:
@@ -783,11 +986,11 @@
 			xbee_handle_status(packet[1]);
 			return 1;
 		case XBEE_FRAME_AT_COMMAND_RESPONSE:
+			//printf("in XBEE_FRAME_AT_COMMAND_RESPONSE case\n");
 			command[0] = packet[2];
 			command[1] = packet[3];
 			command[2] = 0;
-			xbee_handle_at_command_response(command,
-				packet[4], packet + 5, len - 5);
+			xbee_handle_at_command_response(command, packet[4], packet + 5, len - 5);
 			return 1;
 	}
 	return 0;
@@ -798,7 +1001,7 @@
  *
  * @param id the new personal area network (PAN) id
  **/
-void xbee_set_pan_id(int id)
+int xbee_set_pan_id(int id)
 {
 	char s[3];
 	s[0] = (id >> 8) & 0xFF;
@@ -805,12 +1008,12 @@
 	s[1] = id & 0xFF;
 	s[2] = 0;
 	xbee_pending_panID = id;
-	xbee_send_modify_at_command("ID", s);
+	return xbee_send_modify_at_command("ID", s);
 }
 
 /**
  * Get the PAN ID for the XBee.
- * 
+ *
  * @return the personal area network id, or
  * XBEE_PAN_DEFAULT if it has not yet been set.
  **/
@@ -822,23 +1025,25 @@
 /**
  * Set the channel the XBee is using.
  *
- * @param channel the channel the XBee will not use, 
+ * @param channel the channel the XBee will not use,
  * between 0x0B and 0x1A
  *
  * @see xbee_get_channel
  **/
-void xbee_set_channel(int channel)
+int xbee_set_channel(int channel)
 {
 	if (channel < 0x0B || channel > 0x1A)
 	{
 		WL_DEBUG_PRINT("Channel out of range.\r\n");
-		return;
+		return -1;
 	}
+
 	char s[3];
 	s[0] = channel & 0xFF;
 	s[1] = 0;
 	xbee_pending_channel = channel;
-	xbee_send_modify_at_command("CH", s);
+
+	return xbee_send_modify_at_command("CH", s);
 }
 
 /**
@@ -871,4 +1076,3 @@
 	xbee_com_port = port;
 }
 #endif
-
