Index: xbee.c
===================================================================
--- xbee.c	(nonexistent)
+++ xbee.c	(revision 1492)
@@ -0,0 +1,1038 @@
+/**
+ * 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
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * 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
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ **/
+
+/**
+ * @file xbee.c
+ * @brief XBee Interface
+ *
+ * Implementation of low level communication with the XBee in API mode.
+ *
+ * @author Brian Coltin, Colony Project, CMU Robotics Club
+ **/
+
+#include <xbee.h>
+#include <wl_defs.h>
+
+#define ROBOT
+#define WL_DEBUG
+#define WL_DEBUG_PRINT( s ) usb_puts( s )
+#define WL_DEBUG_PRINT_INT( i ) usb_puti(i)
+
+#ifndef ROBOT
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <errno.h>
+#include <termios.h>
+
+#else
+
+#include <serial.h>
+#include <avr/interrupt.h>
+
+#endif
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#define XBEE_FRAME_START 0x7E
+#define XBEE_GET_PACKET_TIMEOUT 1000
+
+/*Frame Types*/
+#define XBEE_FRAME_STATUS 0x8A
+#define XBEE_FRAME_AT_COMMAND 0x08
+#define XBEE_FRAME_AT_COMMAND_RESPONSE 0x88
+#define XBEE_FRAME_TX_REQUEST_64 0x00
+#define XBEE_FRAME_TX_REQUEST_16 0x01
+#define XBEE_FRAME_TX_STATUS XBEE_TX_STATUS
+#define XBEE_FRAME_RX_64 0x80
+#define XBEE_FRAME_RX_16 XBEE_RX
+
+/*Internal Function Prototypes*/
+
+/*I/O Functions*/
+static int xbee_send(char* buf, int size);
+static int xbee_send_string(char* c);
+
+#ifndef ROBOT
+static int xbee_read(char* buf, int size);
+#endif
+
+/*Command Mode Functions
+ * Called during initialization.
+ */
+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*/
+
+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);
+int xbee_send_read_at_command(char* command);
+static int xbee_send_modify_at_command(char* command, char* value);
+
+/*Global Variables*/
+
+#ifndef ROBOT
+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?
+#define XBEE_BUFFER_SIZE	128
+#define PACKET_BUFFER_SIZE	108
+// a buffer for data received from the XBee
+char arrival_buf[XBEE_BUFFER_SIZE];
+// location of last unread byte in buffer
+volatile int buffer_last = 0;
+// first unread byte in buffer
+volatile int buffer_first = 0;
+
+
+//used to store packets as they are read
+static char xbee_buf[PACKET_BUFFER_SIZE];
+static int currentBufPos = 0;
+
+//XBee status
+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;
+
+/*Function Implementations*/
+
+#ifdef ROBOT
+
+/**
+ * Interrupt for the robot. Adds bytes received from the xbee
+ * to the buffer.
+ **/
+#ifndef FIREFLY
+ISR(USART1_RX_vect)
+{
+	char c = UDR1;
+	arrival_buf[buffer_last] = c;
+	int t = buffer_last + 1;
+	if (t == XBEE_BUFFER_SIZE)
+		t = 0;
+	if (t == buffer_first)
+	{
+		WL_DEBUG_PRINT("\nOut of space in buffer.\n");
+	}
+	buffer_last = t;
+}
+#else
+SIGNAL(SIG_USART0_RECV)
+{
+	char c = UDR0;
+	arrival_buf[buffer_last] = c;
+	int t = buffer_last + 1;
+	if (t == XBEE_BUFFER_SIZE)
+		t = 0;
+	if (t == buffer_first)
+	{
+		WL_DEBUG_PRINT("Out of space in buffer.\n");
+	}
+	buffer_last = t;
+}
+#endif
+
+#else
+
+// Computer code
+
+/**
+ * Thread that listens to the xbee.
+ **/
+static void* listen_to_xbee(void* x)
+{
+	char c;
+	while (1)
+	{
+		if (xbee_read(&c, 1) != 0) {
+			WL_DEBUG_PRINT("xbee_read failed.\n");
+			return NULL;
+		}
+
+		arrival_buf[buffer_last] = c;
+		int t = buffer_last + 1;
+		if (t == XBEE_BUFFER_SIZE)
+			t = 0;
+		if (t == buffer_first)
+		{
+			WL_DEBUG_PRINT("Out of space in buffer.\n");
+		}
+		buffer_last = t;
+
+		usleep(1000);
+	}
+
+	return NULL;
+}
+
+#endif
+
+/**
+ * Initializes the XBee library so that other functions may be used.
+ **/
+int xbee_lib_init()
+{
+	WL_DEBUG_PRINT("in xbee_init\n");
+#ifdef ROBOT
+
+	//enable the receiving interrupt
+#ifdef FIREFLY
+	UCSR0B |= _BV(RXCIE) | _BV(RXEN);
+#else
+#ifdef BAYBOARD
+	UCSR1B |= _BV(RXCIE1);
+#else
+	UCSR1B |= _BV(RXCIE);
+#endif
+#endif
+	sei();
+#else
+	xbee_stream = open(xbee_com_port, O_RDWR);
+	if (xbee_stream == -1/* || lockf(xbee_stream, F_TEST, 0) != 0*/)
+	{
+		WL_DEBUG_PRINT("Failed to open connection to XBee on port ");
+		WL_DEBUG_PRINT_INT(xbee_com_port);
+		WL_DEBUG_PRINT(".\n");
+		return -1;
+	} else {
+	  WL_DEBUG_PRINT("Successfully opened connection to XBee on port ");
+		WL_DEBUG_PRINT_INT(xbee_com_port);
+		WL_DEBUG_PRINT(".\n");
+	}
+
+	// set baud rate, etc. correctly
+	struct termios options;
+
+	tcgetattr(xbee_stream, &options);
+	cfsetispeed(&options, B9600);
+	cfsetospeed(&options, B9600);
+	options.c_iflag &= ~ICRNL;
+	options.c_oflag &= ~OCRNL;
+	options.c_cflag |= (CLOCAL | CREAD);
+	options.c_cflag &= ~PARENB;
+	options.c_cflag &= ~CSTOPB;
+	options.c_cflag &= ~CSIZE;
+	options.c_cflag |= CS8;
+	options.c_lflag &= ~ICANON;
+	options.c_cc[VMIN] = 1;
+	options.c_cc[VTIME] = 50;
+
+	if (tcsetattr(xbee_stream, TCSANOW, &options))
+	{
+		WL_DEBUG_PRINT("Error setting attributes.\n");
+		return -1;
+	}
+
+	xbee_listen_thread = (pthread_t*)malloc(sizeof(pthread_t));
+	if (xbee_listen_thread == NULL)
+	{
+		WL_DEBUG_PRINT("Malloc failed.\n");
+		return -1;
+	}
+
+	int ret = pthread_create(xbee_listen_thread, NULL, listen_to_xbee, NULL);
+	if (ret)
+	{
+		WL_DEBUG_PRINT("Failed to create listener thread.\n");
+		return -1;
+	}
+#endif
+
+	WL_DEBUG_PRINT("Entering command mode.\n");
+
+	if (xbee_enter_command_mode() != 0) {
+		return -1;
+	}
+
+	WL_DEBUG_PRINT("Entered command mode.\n");
+
+	if (xbee_enter_api_mode() != 0) {
+		return -1;
+	}
+
+	WL_DEBUG_PRINT("Entered api mode.\n");
+
+	if (xbee_exit_command_mode() != 0) {
+	     return -1;
+	}
+	
+	WL_DEBUG_PRINT("Left command mode.\n");
+
+	if (xbee_send_read_at_command("MY")) {
+		return -1;
+	}
+	WL_DEBUG_PRINT("Getting ATMY address.\n");
+
+#ifndef ROBOT
+	int i;
+	for (i = 0; xbee_address == 0 && i < XBEE_GET_PACKET_TIMEOUT; i++) {
+	  ret = xbee_get_packet(NULL);
+
+	  usleep(1000);
+	  
+/* 	  if (ret == -1) { */
+/* 	    WL_DEBUG_PRINT("xbee_get_packet(NULL) failed.\n"); */
+/* 	    return -1; */
+/* 	  } */
+	}
+#else
+	//wait to return until the address is set
+  //TODO: this shouldn't wait indefinitely.  There should be some sort of reasonable timeout
+  // so if the address is never set right, an error can be returned instead of having the
+  // robot hang forever
+	while (xbee_address == 0) {
+	  xbee_get_packet(NULL);
+	}
+#endif
+	WL_DEBUG_PRINT("Got ATMY address.\n");
+
+#ifndef ROBOT
+	if (i == XBEE_GET_PACKET_TIMEOUT) { // We timed-out.
+
+	  WL_DEBUG_PRINT("xbee_get_packet timed out.\n");
+	  return -1;
+	} else {
+	  return 0;
+	}
+#else
+	return 0;
+#endif
+}
+
+/**
+ * Call when finished using the XBee library. This releases
+ * all sued resources.
+ **/
+void xbee_terminate()
+{
+	#ifndef ROBOT
+	pthread_cancel(*xbee_listen_thread);
+    pthread_join(*xbee_listen_thread, NULL);
+	free(xbee_listen_thread);
+	lockf(xbee_stream, F_ULOCK, 0);
+	close(xbee_stream);
+  #else
+  xbee_exit_api_mode();
+	#endif
+}
+
+/**
+ * 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
+ **/
+static int xbee_send(char* buf, int size)
+{
+#ifdef ROBOT
+	int i;
+	for (i = 0; i < size; i++) {
+		xbee_putc(buf[i]);
+	}
+
+	return 0;
+
+#else
+
+	int ret = write(xbee_stream, buf, size);
+	//success
+	if (ret == size)
+		return 0;
+	if (ret == -1)
+	{
+		//interrupted by system signal, probably timer interrupt.
+		//just try again
+		if (errno == 4)
+		{
+			return xbee_send(buf, size);
+		}
+		WL_DEBUG_PRINT("Failed to write to xbee\r\n");
+		return -1;
+	}
+
+	//write was interrupted after writing ret bytes
+	return xbee_send(buf + ret, size - ret);
+#endif
+}
+
+/**
+ * Sends a string to the XBee.
+ *
+ * @param c the string to send to the XBEE
+ **/
+static int xbee_send_string(char* c)
+{
+	return xbee_send(c, strlen(c));
+}
+
+#ifndef ROBOT
+static int xbee_read(char* buf, int size)
+{
+	if (read(xbee_stream, buf, size) == -1) {
+		WL_DEBUG_PRINT("Failed to read from xbee.\r\n");
+		return -1;
+	}
+
+	return 0;
+}
+#endif
+
+/**
+ * Enter into command mode.
+ **/
+static int xbee_enter_command_mode()
+{
+	if (xbee_send_string("+++") != 0) {
+		return -1;
+	}
+
+	if (xbee_wait_for_ok() != 0) {
+	  return -1;
+	}
+	  return 0;
+}
+
+/**
+ * Exit from command mode.
+ **/
+static int xbee_exit_command_mode()
+{
+	if (xbee_send_string("ATCN\r") != 0) {
+		return -1;
+	}
+
+	xbee_wait_for_ok();
+
+	return 0;
+}
+
+/**
+ * Enter API mode.
+ **/
+static int xbee_enter_api_mode()
+{
+	if (xbee_send_string("ATAP 1\r") != 0) {
+		return -1;
+	}
+	xbee_wait_for_ok();
+
+	return 0;
+}
+
+/**
+ * Exit API mode.
+ **/
+static int xbee_exit_api_mode()
+{
+	if (xbee_send_modify_at_command("AP","0") != 0) {
+		return -1;
+	}
+
+	return 0;
+}
+
+/**
+ * Wait until the string "OK\r" is received from the XBee.
+ **/
+static int xbee_wait_for_ok()
+{
+	return xbee_wait_for_string("OK\r", 3);
+}
+
+/**
+ * Delay until the specified string is received from
+ * the XBee. Discards all other XBee data.
+ *
+ * ********* Robot often hangs here ****************
+ *
+ * @param s the string to receive
+ * @param len the length of the string
+ **/
+static int xbee_wait_for_string(char* s, int len)
+{
+	char* curr = s;
+	while (curr - s < len) {
+    WL_DEBUG_PRINT("waiting for string\r\n");
+		// check if buffer is empty
+		if (buffer_last != buffer_first) {
+			char c = arrival_buf[buffer_first++];
+			if (buffer_first == XBEE_BUFFER_SIZE) {
+				buffer_first = 0;
+			}
+
+			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;
+}
+
+/**
+ * Verifies that the packets checksum is correct.
+ * (If the checksum is correct, the sum of the bytes
+ * is 0xFF.)
+ *
+ * @param packet the packet received. This includes the first
+ * three bytes, which are header information from the XBee.
+ *
+ * @param len The length of the packet received from the XBee
+ *
+ * @return 0 if the checksum is incorrect, nonzero
+ * otherwise
+ **/
+int xbee_verify_checksum(char* packet, int len)
+{
+	unsigned char sum = 0;
+	int i;
+	for (i = 3; i < len; i++)
+		sum += (unsigned char)packet[i];
+	return sum == 0xFF;
+}
+
+/**
+ * Returns the checksum of the given packet.
+ *
+ * @param buf the data for the packet to send
+ * @param len the length of the packet in bytes
+ *
+ * @return the checksum of the packet, which will
+ * become the last byte sent in the packet
+ **/
+char xbee_compute_checksum(char* buf, int len)
+{
+	int i;
+	unsigned char sum = 0;
+	for (i = 0; i < len; i++)
+		sum += (unsigned char)buf[i];
+	return 0xFF - sum;
+}
+
+/**
+ * Adds header information and checksum to the given
+ * packet and sends it. Header information includes
+ * XBEE_FRAME_START and the packet length, as two bytes.
+ *
+ * @param buf the packet data
+ * @param len the size in bytes of the packet data
+ *
+ **/
+static int xbee_send_frame(char* buf, int 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);
+
+	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;
+}
+
+/**
+ * Sends an AT command to read a parameter.
+ *
+ * @param command the AT command to send. For exmaple,
+ * use ID to read the PAN ID and MY to return the XBee ID.
+ * See the XBee reference guide for a complete listing.
+ **/
+int xbee_send_read_at_command(char* command)
+{
+	return xbee_send_modify_at_command(command, NULL);
+}
+
+/**
+ * Sends the given AT command.
+ *
+ * @param command the AT command to send (e.g., MY, ID)
+ * @param value the value to pass as a parameter
+ * (or NULL if there is no parameter)
+ **/
+static int xbee_send_modify_at_command(char* command, char* value)
+{
+	char buf[16];
+	int i;
+
+	buf[0] = XBEE_FRAME_AT_COMMAND;
+	buf[1] = 1;
+	buf[2] = command[0];
+	buf[3] = command[1];
+	int valueLen = 0;
+	if (value != NULL)
+	{
+		valueLen = strlen(value);
+		if (valueLen > 8)
+		{
+			WL_DEBUG_PRINT("AT Command too large.\r\n");
+			return -1;
+		}
+
+		for (i = 0; i < valueLen; i++) {
+			buf[4 + i] = value[i];
+		}
+	}
+
+	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_BROADCAST_ALL_PANS
+ *
+ * @param frame the frame number to associate this packet
+ * with. This will be used to identify the response when
+ * the XBee alerts us as to whether or not our message
+ * was received.
+ **/
+int xbee_send_packet(char* packet, int len, int dest, char options, char frame)
+{
+	char buf[5];
+	char prefix[3];
+	int i;
+	unsigned char checksum = 0;
+
+	if (len > 100)
+	{
+		WL_DEBUG_PRINT("Packet is too large.\r\n");
+		return -1;
+	}
+
+	//data for sending request
+	buf[0] = XBEE_FRAME_TX_REQUEST_16;
+	buf[1] = frame;
+	buf[2] = (dest >> 8) & 0xFF;
+	buf[3] = dest & 0xFF;
+	buf[4] = options;
+
+	//packet prefix, do this here so we don't need an extra buffer
+	prefix[0] = XBEE_FRAME_START;
+	prefix[1] = ((5 + len) & 0xFF00) >> 8;
+	prefix[2] = (5 + len) & 0xFF;
+
+	for (i = 0; i < 5; i++)
+		checksum += (unsigned char)buf[i];
+	for (i = 0; i < len; i++)
+		checksum += (unsigned char)packet[i];
+	checksum = 0xFF - checksum;
+
+	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;
+}
+
+/**
+ * Reads a packet received from the XBee. This function
+ * is non-blocking. The resulting packet is stored in dest.
+ * Only returns transmission response packets and
+ * received packets. The returned packet does not include
+ * header information or the checksum. This method also
+ * handles special packets dealt with by the XBee library,
+ * and so should be called frequently while the XBee is in
+ * use.<br><br>
+ *
+ * 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,
+ * respectively.<br><br>
+ *
+ * For a status response packet:<br>
+ * 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
+ * transmitting the packet.<br><br>
+ *
+ * For a received packet:<br>
+ * The first byte will be XBEE_RX.<br>
+ * The second and third bytes will be the 16-bit
+ * address of the packet's sender.<br>
+ * 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
+ **/
+int xbee_get_packet(unsigned char* dest)
+{
+     int ret;
+	//start reading a packet with XBEE_FRAME_START
+	if (currentBufPos == 0)
+	{
+		do
+		{
+			if (buffer_first == XBEE_BUFFER_SIZE)
+				buffer_first = 0;
+			// check if buffer is empty
+			if (buffer_first == buffer_last) {
+				return -1;
+			}
+		} 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) {
+		len = (int)xbee_buf[2] + ((int)xbee_buf[1] << 8);
+	}
+
+	while (len == -1 //packet length has not been read yet
+		|| currentBufPos < len + 4)
+	{
+		if (currentBufPos == 3)
+		{
+			len = (int)xbee_buf[2] + ((int)xbee_buf[1] << 8);
+			if (len > 120)
+			{
+				WL_DEBUG_PRINT("Packet too large. Probably error in XBee transmission.\n");
+				currentBufPos = 0;
+				return -1;
+			}
+		}
+
+		// check if buffer is empty
+		if (buffer_first == buffer_last) {
+			return -1;
+		}
+		xbee_buf[currentBufPos++] = arrival_buf[buffer_first++];
+		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");
+		return -1;
+	}
+
+	//we will take care of the packet
+	
+	ret = xbee_handle_packet(xbee_buf+3, len);
+	if (ret == 1) {
+		return 3;
+	}
+	
+	if (dest == NULL) {
+		return -1;
+	}
+
+	int i;
+	for (i = 3; i < len + 3; i++) {
+		dest[i - 3] = xbee_buf[i];
+	}
+	return len;
+}
+
+/**
+ * Handles modem status packets.
+ *
+ * @param status the type of status packet received.
+ **/
+void xbee_handle_status(char status)
+{
+	switch (status)
+	{
+		case 0:
+			WL_DEBUG_PRINT("XBee hardware reset.\r\n");
+			break;
+		case 1:
+			WL_DEBUG_PRINT("Watchdog timer reset.\r\n");
+			break;
+		case 2:
+			WL_DEBUG_PRINT("Associated.\r\n");
+			break;
+		case 3:
+			WL_DEBUG_PRINT("Disassociated.\r\n");
+			break;
+		case 4:
+			WL_DEBUG_PRINT("Synchronization lost.\r\n");
+			break;
+		case 5:
+			WL_DEBUG_PRINT("Coordinator realignment.\r\n");
+			break;
+		case 6:
+			WL_DEBUG_PRINT("Coordinator started.\r\n");
+			break;
+	}
+}
+
+/**
+ * Handles AT command response packets.
+ * @param command the two character AT command, e.g. MY or ID
+ * @param result 0 for success, 1 for an error
+ * @param extra the hex value of the requested register
+ * @param extraLen the length in bytes of extra
+ **/
+static void xbee_handle_at_command_response(char* command, char result, char* extra, int extraLen)
+{
+	if (result == 1)
+	{
+		WL_DEBUG_PRINT("Error with AT");
+		WL_DEBUG_PRINT(command);
+		WL_DEBUG_PRINT(" packet.\r\n");
+	}
+	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;
+		WL_DEBUG_PRINT("PAN ID set to ");
+		WL_DEBUG_PRINT_INT(xbee_panID);
+		WL_DEBUG_PRINT(".\r\n");
+		return;
+	}
+
+	if (command[0] == 'C' && command[1] == 'H')
+	{
+		xbee_channel = xbee_pending_channel;
+		WL_DEBUG_PRINT("Channel set to ");
+		WL_DEBUG_PRINT_INT(xbee_channel);
+		WL_DEBUG_PRINT(".\r\n");
+		return;
+	}
+  
+  if (command[0] == 'N' && command[1] == 'D')
+	{
+		xbee_channel = xbee_pending_channel;
+		WL_DEBUG_PRINT("extra=");
+    WL_DEBUG_PRINT(extra);
+		WL_DEBUG_PRINT("\r\n");
+		return;
+	}
+
+	if (command[0] == 'M' && command[1] == 'Y' && extraLen != 0)
+	{
+		xbee_address = 0;
+		int i;
+		for (i = 0; i < extraLen; i++) {
+			xbee_address = (xbee_address << 8) + extra[i];
+		}
+
+		WL_DEBUG_PRINT("XBee address is ");
+		WL_DEBUG_PRINT_INT(xbee_address);
+		WL_DEBUG_PRINT(".\r\n");
+
+		if (xbee_address == 0)
+		{
+			WL_DEBUG_PRINT("XBee 16-bit address must be set using ATMY.\r\n");
+			#ifndef ROBOT
+			exit(0);
+			#endif
+		}
+	}
+}
+
+/**
+ * Attempts to handle the packet if it is dealt with
+ * by the library.
+ * We will handle the following packet types:
+ *    Modem Status
+ *    AT Command Response
+ *
+ * @param packet the packet to handle
+ * @param len the length of the packet
+ *
+ * @return 1 if we have handled the packet, 0 otherwise
+ */
+static int xbee_handle_packet(char* packet, int len)
+{
+
+	char command[3] = {1, 2, 3};
+	if (len <= 0) //this should not happend
+	{
+		WL_DEBUG_PRINT("Non-positive packet length.\r\n");
+		return 0;
+	}
+
+	switch ((unsigned char)packet[0]) //packet type
+	{
+		case XBEE_FRAME_STATUS:
+			xbee_handle_status(packet[1]);
+			return 1;
+		case XBEE_FRAME_AT_COMMAND_RESPONSE:
+			command[0] = packet[2];
+			command[1] = packet[3];
+			command[2] = 0;
+			xbee_handle_at_command_response(command, packet[4], packet + 5, len - 5);
+			return 1;
+	}
+	return 0;
+}
+
+/**
+ * Sets the personal area network id.
+ *
+ * @param id the new personal area network (PAN) id
+ **/
+int xbee_set_pan_id(int id)
+{
+	char s[3];
+	s[0] = (id >> 8) & 0xFF;
+	s[1] = id & 0xFF;
+	s[2] = 0;
+	xbee_pending_panID = id;
+	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.
+ **/
+unsigned int xbee_get_pan_id()
+{
+	return xbee_panID;
+}
+
+/**
+ * Set the channel the XBee is using.
+ *
+ * @param channel the channel the XBee will not use,
+ * between 0x0B and 0x1A
+ *
+ * @see xbee_get_channel
+ **/
+int xbee_set_channel(int channel)
+{
+	if (channel < 0x0B || channel > 0x1A)
+	{
+		WL_DEBUG_PRINT("Channel out of range.\r\n");
+		return -1;
+	}
+
+	char s[3];
+	s[0] = channel & 0xFF;
+	s[1] = 0;
+	xbee_pending_channel = channel;
+
+	return xbee_send_modify_at_command("CH", s);
+}
+
+/**
+ * Returns the channel which the XBee is currently using.
+ *
+ * @return the channel the XBee is using
+ *
+ * @see xbee_set_channel
+ **/
+int xbee_get_channel(void)
+{
+	return xbee_channel;
+}
+
+/**
+ * Get the 16-bit address of the XBee.
+ * This is used to specify who to send messages to
+ * and who messages are from.
+ *
+ * @return the 16-bit address of the XBee.
+ **/
+unsigned int xbee_get_address()
+{
+	return xbee_address;
+}
+
+#ifndef ROBOT
+void xbee_set_com_port(char* port)
+{
+	xbee_com_port = port;
+}
+#endif
