Index: branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/wl_recharge_group.c
===================================================================
--- branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/wl_recharge_group.c	(revision 339)
+++ branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/wl_recharge_group.c	(revision 340)
@@ -353,6 +353,10 @@
 			if (recharge_state == NOT_RECHARGING)
 				wl_recharge_begin();
 			break;
+		case 10:
+			if (recharge_state == DOCKED)
+				wl_recharge_depart();
+			break;
 		default:
 			RECHARGE_DEBUG_PRINT("Error packet of unknown type received.\n");
 			break;
Index: branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/departing.c
===================================================================
--- branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/departing.c	(revision 339)
+++ branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/departing.c	(revision 340)
@@ -31,7 +31,6 @@
  **/
 int depart_station(void)
 {
-	//TODO: use rangefinders to depart
 	int front_distance = range_read_distance(IR2);
 
 	if(front_distance != -1 && front_distance < 160)
Index: branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/main.c
===================================================================
--- branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/main.c	(revision 339)
+++ branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/main.c	(revision 340)
@@ -18,23 +18,19 @@
 	wl_init();
 	usb_puts("Wireless initialized!\n");
 	wl_set_channel(0xE);
-	//orb_set_color(YELLOW);
 	wl_token_ring_register();
-	//wl_token_ring_set_bom_functions(do_nothing2, do_nothing2, get_nothing2); 
 	wl_token_ring_join();
-	//usb_puts("Wireless initialized.\n");
 	recharge_init();
-	//usb_puts("Recharging initialized.\n");
-	//run_around_init();
+	usb_puts("Recharging initialized.\n");
+	run_around_init();
 	while (1)
 	{
 		wl_do();
 		int charging = recharge_do();
-		//if (!charging)
-		//{
-			//analog8(IR1);
-			//run_around_FSM();
-		//}
+		if (!charging)
+		{
+			run_around_FSM();
+		}
 	}
 	
 	return 0;
Index: branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/recharge_defs.h
===================================================================
--- branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/recharge_defs.h	(revision 339)
+++ branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/recharge_defs.h	(revision 340)
@@ -1,4 +1,4 @@
-#define RECHARGE_DEBUG
+//#define RECHARGE_DEBUG
 
 #ifdef RECHARGE_DEBUG
 #define RECHARGE_DEBUG_PRINT( s ) usb_puts( s )
Index: branches/autonomous_recharging/code/projects/libwireless/test/test.c
===================================================================
--- branches/autonomous_recharging/code/projects/libwireless/test/test.c	(revision 339)
+++ branches/autonomous_recharging/code/projects/libwireless/test/test.c	(revision 340)
@@ -10,13 +10,18 @@
 
 int main(int argc, char** argv)
 {
-	int robot = atoi(argv[1]);
+	char c = argv[1][0];
+	int robot = atoi(argv[2]);
+	wl_set_com_port("/dev/ttyUSB0");
 	wl_init();
 	wl_set_channel(0xE);
 	printf("Wireless initialized.\n");
 	printf("Packet groups initialized.\n");
 
-	wl_send_robot_to_robot_global_packet(3, 9, NULL, 0, robot, 0);
+	if (c == 'g')
+		wl_send_robot_to_robot_global_packet(3, 9, NULL, 0, robot, 0);
+	if (c == 'd')
+		wl_send_robot_to_robot_global_packet(3, 10, NULL, 0, robot, 0);
 	
 	wl_terminate();
 	
Index: branches/autonomous_recharging/code/projects/libwireless/test/Makefile
===================================================================
--- branches/autonomous_recharging/code/projects/libwireless/test/Makefile	(revision 339)
+++ branches/autonomous_recharging/code/projects/libwireless/test/Makefile	(revision 340)
@@ -4,7 +4,7 @@
 all: test
 
 test: *.c ../lib/*.c 
-	$(CC) $(CFLAGS) *.c -L../lib -o test -pthread -lrt -lwireless
+	$(CC) $(CFLAGS) *.c -L../lib -I../lib -o test -pthread -lrt -lwireless
 
 clean:
 	rm *.o test ../lib/*.o
Index: branches/autonomous_recharging/code/projects/libwireless/lib/wl_token_ring.c
===================================================================
--- branches/autonomous_recharging/code/projects/libwireless/lib/wl_token_ring.c	(revision 339)
+++ branches/autonomous_recharging/code/projects/libwireless/lib/wl_token_ring.c	(revision 340)
@@ -1,3 +1,37 @@
+/**
+ * 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 wl_token_ring.c
+ * @brief Token Ring Implementation
+ *
+ * Implementation of the token ring packet group.
+ *
+ * @author Brian Coltin, Colony Project, CMU Robotics Club
+ **/
+
 #include <wl_token_ring.h>
 
 #include <stdlib.h>
@@ -6,7 +40,6 @@
 #include <wl_defs.h>
 #include <wireless.h>
 #include <sensor_matrix.h>
-#include <queue.h>
 
 #ifdef ROBOT
 #ifndef FIREFLY
@@ -44,8 +77,6 @@
 
 /*Packet Handling Routines*/
 void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength);
-void wl_token_interrupt_request_receive(int source, int robot);
-void wl_token_interrupt_pass_receive(int source, int robot);
 void wl_token_bom_on_receive(int source);
 void wl_token_join_receive(int source);
 void wl_token_join_accept_receive(int source);
@@ -69,12 +100,13 @@
 int deathDelay = -1;
 //the counter for joining, before we form our own token ring
 int joinDelay = -1;
-//queue containing ids of interruption requests
-Queue* interrupting = NULL;
 
 //current robot to check in the iterator
 int iteratorCount = 0;
 
+// the amount of time a robot has had its BOM on for
+int bom_on_count = 0;
+
 void do_nothing(void) {}
 int get_nothing(void) {return -1;}
 
@@ -116,7 +148,6 @@
 	}
 	
 	sensorMatrix = sensor_matrix_create();
-	interrupting = queue_create();
 	//add ourselves to the sensor matrix
 	sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);
 
@@ -157,7 +188,6 @@
 void wl_token_ring_cleanup()
 {
 	sensor_matrix_destroy(sensorMatrix);
-	queue_destroy(interrupting);
 }
 
 /**
@@ -176,6 +206,8 @@
 			WL_DEBUG_PRINT("Robot ");
 			WL_DEBUG_PRINT_INT(wl_token_next_robot);
 			WL_DEBUG_PRINT(" has died.\r\n");
+			wl_token_next_robot = -1;
+			deathDelay = DEATH_DELAY;
 		}
 		
 		// we may have been dropped from the ring when this is received
@@ -209,6 +241,8 @@
 		deathDelay--;
 	if (joinDelay >= 0)
 		joinDelay--;
+	if (bom_on_count >= 0)
+		bom_on_count++;
 }
 
 /**
@@ -249,12 +283,6 @@
 			//add the robot to the sensor matrix if it is not already there
 			wl_token_bom_on_receive(source);
 			break;
-		case WL_TOKEN_INTERRUPT_REQUEST:
-			wl_token_interrupt_request_receive(source, packet[0]);
-			break;
-		case WL_TOKEN_INTERRUPT_PASS:
-			wl_token_interrupt_pass_receive(source, packet[0]);
-			break;
 		case WL_TOKEN_JOIN:
 			wl_token_join_receive(source);
 			break;
@@ -276,7 +304,7 @@
 {
 	WL_DEBUG_PRINT("Joining the token ring.\r\n");
 	ringState = JOINING;
-	joinDelay = JOIN_DELAY;
+	joinDelay = DEATH_DELAY * 2;
 	wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN,
 		NULL, 0, 0);
 }
@@ -292,21 +320,6 @@
 }
 
 /**
- * Requests that the specified robot be given the token and
- * allowed to flash its BOM. After its BOM is flashed, the
- * token will return to the robot who sent it.
- *
- * @param robot the ID of the robot which should flash its BOM
- **/
-void wl_token_request(int robot)
-{
-	char buf[1];
-	buf[0] = robot;
-	wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_INTERRUPT_REQUEST,
-		buf, 1, 0);
-}
-
-/**
  * Returns the BOM reading robot source has for robot dest.
  *
  * @param source the robot that made the BOM reading
@@ -317,7 +330,10 @@
  **/
 int wl_token_get_sensor_reading(int source, int dest)
 {
-	return sensor_matrix_get_reading(sensorMatrix, source, dest);
+	if (wl_token_is_robot_in_ring(dest) &&
+			(source == wl_get_xbee_id() || wl_token_is_robot_in_ring(source)))
+		return sensor_matrix_get_reading(sensorMatrix, source, dest);
+	return -1;
 }
 
 /**
@@ -344,9 +360,21 @@
 void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength)
 {
 	int i, j;
+
+	// this prevents two tokens from being passed around at a time (second clause is in case we are joining)
+	if (source != wl_token_next_robot && bom_on_count <= DEATH_DELAY / 2 &&
+		ringState != ACCEPTED)
+	{
+		WL_DEBUG_PRINT("Received token pass when a robot should not have died yet.\n");
+		WL_DEBUG_PRINT("There are probably two tokens going around, packet ignored.\n");
+		return;
+	}
+
+	bom_on_count = -1;
 	deathDelay = -1;
-
-	WL_DEBUG_PRINT("Received the token, next robot is ");
+	WL_DEBUG_PRINT("Received the token from robot");
+	WL_DEBUG_PRINT_INT(source);
+	WL_DEBUG_PRINT(", next robot is ");
 	WL_DEBUG_PRINT_INT((int)nextRobot);
 	WL_DEBUG_PRINT(" \r\n");
 	sensor_matrix_set_in_ring(sensorMatrix, source, 1);
@@ -516,6 +544,9 @@
 	WL_DEBUG_PRINT("Robot ");
 	WL_DEBUG_PRINT_INT(source);
 	WL_DEBUG_PRINT(" has flashed its bom.\r\n");
+	
+	bom_on_count = 0;
+
 	sensor_matrix_set_reading(sensorMatrix, wl_get_xbee_id(), 
 		source, get_max_bom_function());
 }
@@ -536,6 +567,7 @@
 			wl_get_xbee_id(), 1);
 		WL_DEBUG_PRINT("Now a member of the token ring.\r\n");
 		ringState = MEMBER;
+		joinDelay = -1;
 	}
 
 	if (ringState == LEAVING || ringState == NONMEMBER)
@@ -548,23 +580,6 @@
 		return;
 	}
 	
-	//check for interruption requests
-	if (queue_size(interrupting) > 0)
-	{
-		char buf[1];
-		buf[0] = (char)(int)queue_remove(interrupting);
-		
-		//in case this robot has requested multiple times
-		queue_remove_all(interrupting, (void*)(int)buf[0]);
-
-		wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_INTERRUPT_PASS,
-			buf, 1, 0);
-
-		deathDelay = DEATH_DELAY;
-		wl_token_next_robot = buf[0];
-		return;
-	}
-
 	WL_DEBUG_PRINT("Our BOM has been flashed.\r\n");
 	wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON,
 		NULL, 0, 0);
@@ -578,6 +593,7 @@
 	if (!sensor_matrix_get_in_ring(sensorMatrix, wl_get_xbee_id()))
 	{
 		WL_DEBUG_PRINT("Removed from sensor matrix while flashing BOM.\r\n");
+		return;
 	}
 	
 	wl_token_pass_token();
@@ -631,8 +647,6 @@
 	WL_DEBUG_PRINT_INT(source);
 	WL_DEBUG_PRINT(" into the token ring.\r\n");
 
-	joinDelay = -1;
-	
 	// the token ring has not started yet
 	if (sensor_matrix_get_joined(sensorMatrix) == 1)
 		wl_token_pass_token();
@@ -650,7 +664,7 @@
 	WL_DEBUG_PRINT("Accepted into the token ring by robot ");
 	WL_DEBUG_PRINT_INT(source);
 	WL_DEBUG_PRINT(".\r\n");
-	joinDelay = -1;
+	joinDelay = JOIN_DELAY;
 	ringState = ACCEPTED;
 	acceptor = source;
 
@@ -659,64 +673,6 @@
 }
 
 /**
- * Called when we receive a packet passing the token and interrupting
- * the token ring.
- * If the token has been passed to us, we flash our BOM
- * and pass it back.
- *
- * @param source the robot who sent the interrupt packet
- * @param robot the robot the token has been passed to
- **/
-void wl_token_interrupt_pass_receive(int source, int robot)
-{
-	if (wl_get_xbee_id() != robot)
-	{
-		queue_remove_all(interrupting, (void*)robot);
-		wl_token_next_robot = robot;
-		deathDelay = DEATH_DELAY + rand() / (RAND_MAX / (2 * DEATH_DELAY));
-		return;
-	}
-	wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON,
-		NULL, 0, 0);
-	
-	bom_on_function();
-	#ifdef ROBOT
-	delay_ms(BOM_DELAY);
-	#endif
-	bom_off_function();
-
-	//we don't include ourself, only if we are in the ring
-	int packetSize = 1 + 2 * (sensor_matrix_get_joined(sensorMatrix) - 1);
-	if (!sensor_matrix_get_in_ring(sensorMatrix, wl_get_xbee_id()))
-		packetSize += 2;
-	char* buf = (char*)malloc(packetSize * sizeof(char));
-	if (!buf)
-	{
-		WL_DEBUG_PRINT("Out of memory - pass_receive.\r\n");
-		return;
-	}
-	
-	//return the token to where we got it from
-	buf[0] = source;
-
-	int i = 0, j = 0;
-	for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++)
-		if (sensor_matrix_get_in_ring(sensorMatrix, i) && i != wl_get_xbee_id())
-		{
-			buf[2*j + 1] = i;
-			buf[2*j + 2] = sensor_matrix_get_reading(sensorMatrix, wl_get_xbee_id(), i);
-			j++;
-		}
-	
-	wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS,
-		buf, packetSize, 0);
-
-	wl_token_next_robot = source;
-	deathDelay = DEATH_DELAY;
-	free(buf);
-}
-
-/**
  * Returns the number of robots in the token ring.
  *
  * @return the number of robots in the token ring
@@ -752,6 +708,7 @@
 		i++;
 	if (i == sensor_matrix_get_size(sensorMatrix))
 		i = -1;
+	iteratorCount = i;
 }
 
 /**
@@ -765,7 +722,7 @@
  **/
 int wl_token_iterator_has_next(void)
 {
-	return iteratorCount == -1;
+	return iteratorCount != -1;
 }
 
 /**
@@ -791,23 +748,22 @@
 }
 
 /**
- * Called when we receive a request to interrupt the token ring.
- * We add the robot to our list of interrupt requests,
- * and will send the token to this robot when we next receive the
- * token, unless someone else does so first.
+ * Returns the number of robots currently in the token ring.
  *
- * @param source the robot requesting interruption
- * @param robt the robot requested to interrupt the token ring
+ * @return the number of robots in the token ring
  **/
-void wl_token_interrupt_request_receive(int source, int robot)
+int wl_token_get_num_robots(void)
 {
-	queue_add(interrupting, (void*)robot);
+	return sensor_matrix_get_joined(sensorMatrix);
 }
 
-int wl_token_get_num_robots(void){
-  return sensor_matrix_get_joined(sensorMatrix);
+/**
+ * Returns the number of robots in the sensor matrix.
+ *
+ * @return the number of robots in the sensor matrix
+ **/
+int wl_token_get_matrix_size(void)
+{
+	return sensor_matrix_get_size(sensorMatrix);
 }
 
-int wl_token_get_matrix_size(void){
-  return sensor_matrix_get_size(sensorMatrix);
-}
Index: branches/autonomous_recharging/code/projects/libwireless/lib/sensor_matrix.h
===================================================================
--- branches/autonomous_recharging/code/projects/libwireless/lib/sensor_matrix.h	(revision 339)
+++ branches/autonomous_recharging/code/projects/libwireless/lib/sensor_matrix.h	(revision 340)
@@ -1,4 +1,29 @@
 /**
+ * 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 sensor_matrix.h
  * @brief Definitions for sensor matrices
  *
@@ -7,6 +32,10 @@
  * @author Brian Coltin, Colony Project, CMU Robotics Club
  **/
 
+#ifndef SENSOR_MATRIX_H
+#define SENSOR_MATRIX_H
+
+
 /**
  * @defgroup sensormatrix Sensor Matrix
  * @brief the robot sensor matrix
@@ -27,7 +56,7 @@
 	/**
 	 * The size of the sensor matrix.
 	**/
-	unsigned int size;
+	int size;
 	/**
 	 * The matrix. Each row represents the readings of one
 	 * robot.
@@ -49,9 +78,9 @@
 /**@brief Destroy a sensor matrix **/
 void sensor_matrix_destroy(SensorMatrix* m);
 /**@brief Add a robot to a sensor matrix **/
-void sensor_matrix_add_robot(SensorMatrix* m, unsigned int id);
+void sensor_matrix_add_robot(SensorMatrix* m, int id);
 /**@brief Remove a robot from a sensor matrix **/
-void sensor_matrix_remove_robot(SensorMatrix* m, unsigned int id);
+void sensor_matrix_remove_robot(SensorMatrix* m, int id);
 /**@brief Set a reading in a sensor matrix **/
 void sensor_matrix_set_reading(SensorMatrix* m, int observer, int robot, int reading);
 /**@brief Get a reading in a sensor matrix **/
@@ -67,3 +96,5 @@
 
 /** @} **/ //end defgroup
 
+
+#endif
Index: branches/autonomous_recharging/code/projects/libwireless/lib/wireless.h
===================================================================
--- branches/autonomous_recharging/code/projects/libwireless/lib/wireless.h	(revision 339)
+++ branches/autonomous_recharging/code/projects/libwireless/lib/wireless.h	(revision 340)
@@ -1,4 +1,29 @@
 /**
+ * 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 wireless.h
  * @brief Contains definitions for the wireless library.
  *
@@ -6,6 +31,9 @@
  *
  * @author Brian Coltin, Colony Project, CMU Robotics Club
  **/
+
+#ifndef WIRELESS_H
+#define WIRELESS_H
  
 //Note: If this is raised above 16, we will need to do
 //something about frame numbers for TX Status packets.
@@ -90,7 +118,7 @@
 } PacketGroupHandler;
 
 /**@brief Initialize the wireless library **/
-void wl_init(void);
+int wl_init(void);
 /**@brief Uninitialize the wireless library **/
 void wl_terminate(void);
 /**@brief Perform wireless library functionality **/
@@ -122,7 +150,11 @@
 /**@brief Get the channel we are using **/
 int wl_get_channel(void);
 /**@brief Get the 16-bit address of the XBee module **/
-unsigned int wl_get_xbee_id(void);
+int wl_get_xbee_id(void);
+/**@brief Set the com port on a computer, undefined on the robot.**/
+void wl_set_com_port(char* port);
 
 /** @} **/ // end defgroup
 
+#endif
+
Index: branches/autonomous_recharging/code/projects/libwireless/lib/xbee.c
===================================================================
--- branches/autonomous_recharging/code/projects/libwireless/lib/xbee.c	(revision 339)
+++ branches/autonomous_recharging/code/projects/libwireless/lib/xbee.c	(revision 340)
@@ -1,3 +1,37 @@
+/**
+ * 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"
 
@@ -7,6 +41,7 @@
 #include <unistd.h>
 #include <pthread.h>
 #include <errno.h>
+#include <termios.h>
 
 #else
 
@@ -19,8 +54,6 @@
 #include <stdlib.h>
 #include <string.h>
 
-#include <queue.h>
-
 #define XBEE_FRAME_START 0x7E
 
 /*Frame Types*/
@@ -49,6 +82,7 @@
 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);
 
@@ -67,12 +101,21 @@
 /*Global Variables*/
 
 #ifndef ROBOT
+char* xbee_com_port = XBEE_PORT_DEFAULT;
 int xbee_stream;
 pthread_t* xbee_listen_thread;
 #endif
 
-Queue* xbee_queue;
+// TODO: is this a good size?
+#define XBEE_BUFFER_SIZE	256
+// 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
 char xbee_buf[128];
 int currentBufPos = 0;
@@ -90,19 +133,35 @@
 
 /**
  * Interrupt for the robot. Adds bytes received from the xbee
- * to the queue.
+ * to the buffer.
  **/
 #ifndef FIREFLY
 ISR(USART1_RX_vect)
 {
 	char c = UDR1;
-	queue_add(xbee_queue, (void*)(int)c);
+	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;
 }
 #else
 SIGNAL(SIG_USART0_RECV)
 {
 	char c = UDR0;
-	queue_add(xbee_queue, (void*)(int)c);
+	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
 
@@ -117,7 +176,15 @@
 	while (1)
 	{
 		xbee_read(&c, 1);
-		queue_add(xbee_queue, (void*)(int)c);
+		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;
 	}
 	return 0;
 }
@@ -126,14 +193,12 @@
 
 /**
  * Initializes the XBee library so that other functions may be used.
- *
- * @param pan_id the PAN to join initially. Use XBEE_PAN_DEFAULT
- * to leave the PAN as it is initially.
  **/
-void xbee_lib_init(void)
+int xbee_lib_init(void)
 {
-	xbee_queue = queue_create();
-	
+	arrival_buf[0] = 'A';
+	arrival_buf[1] = 'A';
+	arrival_buf[2] = 'A';
 	#ifdef ROBOT
 
 	//enable the receiving interrupt
@@ -144,26 +209,55 @@
 	#endif
 	sei();
 	#else
-	xbee_stream = open("/dev/ttyUSB0", O_RDWR);
-	if (xbee_stream == -1 || lockf(xbee_stream, F_TEST, 0) != 0)
-		xbee_stream = open("/dev/ttyUSB1", O_RDWR);
-	if (xbee_stream == -1 || lockf(xbee_stream, F_TEST, 0) != 0)
+	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*/)
 	{
-		printf("Failed to open connection to XBee.\r\n");
-		exit(0);
+		printf("Failed to open connection to XBee on port %s\r\n", xbee_com_port);
+		return -1;
 	}
-	lockf(xbee_stream, F_LOCK, 0);
 	
+	// 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))
+	{
+		fprintf(stderr, "Error setting attributes.\n");
+		return -1;
+	}
+
+	//lockf(xbee_stream, F_LOCK, 0);
+	
 	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, 
+	int ret = pthread_create(xbee_listen_thread, NULL,
 		listen_to_xbee, NULL);
 	if (ret)
 	{
-		printf("Failed to create listener thread.\r\n");
-		exit(0);
+		fprintf(stderr, "Failed to create listener thread.\r\n");
+		return -1;
 	}
+	
 	#endif
 	xbee_enter_command_mode();
 	xbee_enter_api_mode();
@@ -172,6 +266,9 @@
 	
 	//wait to return until the address is set
 	while (xbee_address == 0) xbee_get_packet(NULL);
+
+
+	return 0;
 }
 
 /**
@@ -186,7 +283,6 @@
 	lockf(xbee_stream, F_ULOCK, 0);
 	close(xbee_stream);
 	#endif
-	queue_destroy(xbee_queue);
 }
 
 /**
@@ -270,6 +366,14 @@
 }
 
 /**
+ * Exit API mode. (warning - does not check for response)
+ **/
+void xbee_exit_api_mode()
+{
+	xbee_send_string("ATAP 0\r");
+}
+
+/**
  * Wait until the string "OK\r" is received from the XBee.
  **/
 void xbee_wait_for_ok()
@@ -289,9 +393,12 @@
 	char* curr = s;
 	while (curr - s < len)
 	{
-		if (queue_is_empty(xbee_queue))
+		// check if buffer is empty
+		if (buffer_last == buffer_first)
 			continue;
-		char c = (char)(int)queue_remove(xbee_queue);
+		char c = arrival_buf[buffer_first++];
+		if (buffer_first == XBEE_BUFFER_SIZE)
+			buffer_first = 0;
 		if (c == *curr)
 			curr++;
 		else
@@ -499,9 +606,16 @@
 	if (currentBufPos == 0)
 	{
 		do
-			if (queue_is_empty(xbee_queue))
+		{
+			if (buffer_first == XBEE_BUFFER_SIZE)
+				buffer_first = 0;
+			// check if buffer is empty
+			if (buffer_first == buffer_last)
 				return -1;
-		while ((char)(int)queue_remove(xbee_queue) != XBEE_FRAME_START);
+		}
+		while (arrival_buf[buffer_first++] != XBEE_FRAME_START);
+		if (buffer_first == XBEE_BUFFER_SIZE)
+			buffer_first = 0;
 		xbee_buf[0] = XBEE_FRAME_START;
 		currentBufPos++;
 	}
@@ -514,10 +628,21 @@
 			|| currentBufPos < len + 4)
 	{
 		if (currentBufPos == 3)
+		{
 			len = (int)xbee_buf[2] + ((int)xbee_buf[1] << 8);
-		if (queue_is_empty(xbee_queue))
+			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++] = (char)(int)queue_remove(xbee_queue);
+		xbee_buf[currentBufPos++] = arrival_buf[buffer_first++];
+		if (buffer_first == XBEE_BUFFER_SIZE)
+			buffer_first = 0;
 	}
 	
 	currentBufPos = 0;
@@ -740,3 +865,10 @@
 	return xbee_address;
 }
 
+#ifndef ROBOT
+void xbee_set_com_port(char* port)
+{
+	xbee_com_port = port;
+}
+#endif
+
Index: branches/autonomous_recharging/code/projects/libwireless/lib/wl_token_ring.h
===================================================================
--- branches/autonomous_recharging/code/projects/libwireless/lib/wl_token_ring.h	(revision 339)
+++ branches/autonomous_recharging/code/projects/libwireless/lib/wl_token_ring.h	(revision 340)
@@ -1,4 +1,29 @@
 /**
+ * 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 wl_token_ring.h
  * @brief Declarations for the token ring packet group
  * 
@@ -7,6 +32,9 @@
  * @author Brian Coltin, Colony Project, CMU Robotics Club
  **/
 
+#ifndef WL_TOKEN_RING_H
+#define WL_TOKEN_RING_H
+
 /**
  * @defgroup tokenring Token Ring
  * @brief Wireless library token ring implementation
@@ -30,8 +58,6 @@
 void wl_token_ring_join(void);
 /**@brief Leave the token ring **/
 void wl_token_ring_leave(void);
-/**@brief Request a BOM flash from the specified robot **/
-void wl_token_request(int robot);
 
 /**@brief Return the number of robots in the token ring **/
 int wl_token_get_robots_in_ring(void);
@@ -56,3 +82,4 @@
 
 /** @} **/ //end token ring group
 
+#endif
Index: branches/autonomous_recharging/code/projects/libwireless/lib/wl_error_group.c
===================================================================
--- branches/autonomous_recharging/code/projects/libwireless/lib/wl_error_group.c	(revision 339)
+++ branches/autonomous_recharging/code/projects/libwireless/lib/wl_error_group.c	(revision 340)
@@ -1,3 +1,37 @@
+/**
+ * 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 wl_error_group.h
+ * @brief Error Packets
+ *
+ * A wireless group for sending error packets.
+ *
+ * @author Brian Coltin, Colony Project, CMU Robotics Club
+ **/
+
 #include "wl_error_group.h"
 
 #include <wireless.h>
Index: branches/autonomous_recharging/code/projects/libwireless/lib/wl_defs.h
===================================================================
--- branches/autonomous_recharging/code/projects/libwireless/lib/wl_defs.h	(revision 339)
+++ branches/autonomous_recharging/code/projects/libwireless/lib/wl_defs.h	(revision 340)
@@ -1,8 +1,46 @@
+/**
+ * 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 wl_defs.h
+ * @brief Definitions for Wireless
+ *
+ * Contains definitions for wireless packet groups, packet types,
+ * debugging information, etc.
+ *
+ * @author Brian Coltin, Colony Project, CMU Robotics Club
+ **/
+
+#ifndef WL_DEFS_H
+#define WL_DEFS_H
+
 //comment out this line if using a computer hooked up to an xbee
 //#define ROBOT
 
 //uncomment this line for debug information
-#define WL_DEBUG
+//#define WL_DEBUG
 
 // Packet Groups and Types
 
@@ -16,14 +54,17 @@
 
 #define WL_TOKEN_PASS 1
 #define WL_TOKEN_BOM_ON 2
-#define WL_TOKEN_INTERRUPT_REQUEST 3
-#define WL_TOKEN_INTERRUPT_PASS 4
-#define WL_TOKEN_JOIN 5
-#define WL_TOKEN_JOIN_ACCEPT 6
+#define WL_TOKEN_JOIN 3
+#define WL_TOKEN_JOIN_ACCEPT 4
 
 // timing constants
+#ifndef FIREFLY
 #define BOM_DELAY 100
-#define DEATH_DELAY 3
+#else
+#define BOM_DELAY 200
+#endif
+
+#define DEATH_DELAY 4
 #define JOIN_DELAY 8
 
 // Recharging group
@@ -63,3 +104,5 @@
 
 #endif
 
+#endif
+
Index: branches/autonomous_recharging/code/projects/libwireless/lib/xbee.h
===================================================================
--- branches/autonomous_recharging/code/projects/libwireless/lib/xbee.h	(revision 339)
+++ branches/autonomous_recharging/code/projects/libwireless/lib/xbee.h	(revision 340)
@@ -1,4 +1,29 @@
 /**
+ * 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.h
  * @brief Contains definitions for using the XBee
  *
@@ -10,7 +35,18 @@
  * @author Brian Coltin, Colony Project, CMU Robotics Club
  **/
 
+#ifndef XBEE_H
+#define XBEE_H
+
 /**
+ * The port to use the XBee from on the computer.
+ * Also, a backup port if the other is used.
+ **/
+#ifndef ROBOT
+#define XBEE_PORT_DEFAULT "/dev/ttyUSB1"
+#endif
+
+/**
  * @defgroup xbee XBee
  * @brief Interface with the XBee module
  *
@@ -38,7 +74,7 @@
 #define XBEE_RX 0x81
 
 /**@brief Initialize the XBee library **/
-void xbee_lib_init(void);
+int xbee_lib_init(void);
 /**@brief Uninitialize the XBee library **/
 void xbee_terminate(void);
 /**@brief Get a packet from the XBee **/
@@ -56,6 +92,9 @@
 int xbee_get_channel(void);
 /**@brief Get the XBee's 16-bit address **/
 unsigned int xbee_get_address(void);
+/**@brief Set the com port on a computer, undefined on the robot**/
+void xbee_set_com_port(char* port);
 
 /**@}**/ //end defgroup
 
+#endif
Index: branches/autonomous_recharging/code/projects/libwireless/lib/Makefile
===================================================================
--- branches/autonomous_recharging/code/projects/libwireless/lib/Makefile	(revision 339)
+++ branches/autonomous_recharging/code/projects/libwireless/lib/Makefile	(revision 340)
@@ -155,7 +155,7 @@
 
 # Combine all necessary flags and optional flags.
 # Add target processor to flags.
-ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS) $(GENDEPFLAGS)
+ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS) -Wall $(GENDEPFLAGS)
 ALL_ASFLAGS = rcs
 
 # Default target.
@@ -176,9 +176,13 @@
 	$(AR) $(ALL_ASFLAGS) $(TARGET).a $(OBJ)
 
 computer:
-	gcc *.c -g -I. -c
+	gcc *.c -Wall -g -I. -c
 	ar rcs $(TARGET).a $(OBJ)
 
+colonet:
+	g++ -Wall -Wshadow -Wextra *.c -g -I. -c
+	ar rcs $(TARGET).a $(OBJ)
+
 # Eye candy.
 # AVR Studio 3.x does not check make's exit code but relies on
 # the following magic strings to be generated by the compile job.
Index: branches/autonomous_recharging/code/projects/libwireless/lib/wl_error_group.h
===================================================================
--- branches/autonomous_recharging/code/projects/libwireless/lib/wl_error_group.h	(revision 339)
+++ branches/autonomous_recharging/code/projects/libwireless/lib/wl_error_group.h	(revision 340)
@@ -1,5 +1,42 @@
 /**
+ * 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 wl_error_group.h
+ * @brief Error Packets
+ *
+ * A packet group for sending error packets.
+ *
+ * @author Brian Coltin, Colony Project, CMU Robotics Club
+ **/
+
+#ifndef WL_ERROR_GROUP_H
+#define WL_ERROR_GROUP_H
+
+/**
+ * @file wl_error_group.h
  * @brief A packet group for error messages.
  *
  * A packet group for sending and receiving error
@@ -26,3 +63,4 @@
 
 /** @} **/ // end defgroup
 
+#endif
Index: branches/autonomous_recharging/code/projects/libwireless/lib/sensor_matrix.c
===================================================================
--- branches/autonomous_recharging/code/projects/libwireless/lib/sensor_matrix.c	(revision 339)
+++ branches/autonomous_recharging/code/projects/libwireless/lib/sensor_matrix.c	(revision 340)
@@ -1,3 +1,37 @@
+/**
+ * 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 sensor_matrix.c
+ * @brief Sensor Matrix implementation
+ *
+ * Implementation of a sensor matrix for storing localization implementation.
+ *
+ * @author Brian Coltin, Colony Project, CMU Robotics Club
+ **/
+
 #include <stdlib.h>
 #include <stdio.h>
 #include <wl_defs.h>
@@ -19,7 +53,7 @@
 	SensorMatrix* m;
 	int i;
 	
-	m = malloc(sizeof(SensorMatrix));
+	m = (SensorMatrix*)malloc(sizeof(SensorMatrix));
 	if (!m)
 	{
 		WL_DEBUG_PRINT("Out of memory - create sensor matrix.\r\n");
@@ -26,8 +60,8 @@
 		return NULL;
 	}
 	m->size = DEFAULT_SENSOR_MATRIX_SIZE;
-	m->matrix = malloc(m->size * sizeof(int*));
-	m->joined = malloc(m->size * sizeof(int));
+	m->matrix = (int**)malloc(m->size * sizeof(int*));
+	m->joined = (int*)malloc(m->size * sizeof(int));
 	m->numJoined = 0;
 	if (!(m->matrix) || !(m->joined))
 	{
@@ -65,7 +99,7 @@
  * @param m the sensor matrix
  * @param id the XBee ID of the robot to add
  **/
-void sensor_matrix_add_robot(SensorMatrix* m, unsigned int id)
+void sensor_matrix_add_robot(SensorMatrix* m, int id)
 {
 	int i;
 	if (id >= m->size)
@@ -73,7 +107,7 @@
 	if (m->matrix[id] != NULL)
 		return;
 	
-	m->matrix[id] = malloc(m->size * sizeof(int));
+	m->matrix[id] = (int*)malloc(m->size * sizeof(int));
 	if (!(m->matrix[id]))
 	{
 		WL_DEBUG_PRINT("Out of memory - add robot.\r\n");
@@ -92,7 +126,7 @@
  * @param m the sensor matrix
  * @param id the XBee ID of the robot to remove
  **/
-void sensor_matrix_remove_robot(SensorMatrix* m, unsigned int id)
+void sensor_matrix_remove_robot(SensorMatrix* m, int id)
 {
 	int i;
 
@@ -125,7 +159,7 @@
 	int i, j;
 	WL_DEBUG_PRINT("Expanding sensor matrix.\r\n");
 	
-	int** tempMatrix = malloc(nextSize * sizeof(int*));
+	int** tempMatrix = (int**)malloc(nextSize * sizeof(int*));
 	if (!tempMatrix)
 	{
 		WL_DEBUG_PRINT("Out of memory - expand matrix.\r\n");
@@ -139,7 +173,7 @@
 	for (i = 0; i < m->size; i++)
 		if (m->matrix[i] != NULL)
 		{
-			tempMatrix[i] = malloc(nextSize * sizeof(int));
+		  tempMatrix[i] = (int *)malloc(nextSize * sizeof(int));
 			if (!tempMatrix[i])
 			{
 				WL_DEBUG_PRINT("Out of memory - expand matrix 2.\r\n");
@@ -158,7 +192,7 @@
 	m->size = nextSize;
 
 	//expand the size of joined
-	int* tempJoined = malloc(nextSize * sizeof(int));
+	int* tempJoined = (int *)malloc(nextSize * sizeof(int));
 	if (!tempJoined)
 	{
 		WL_DEBUG_PRINT("Out of memory - expand matrix 3.\r\n");
Index: branches/autonomous_recharging/code/projects/libwireless/lib/wireless.c
===================================================================
--- branches/autonomous_recharging/code/projects/libwireless/lib/wireless.c	(revision 339)
+++ branches/autonomous_recharging/code/projects/libwireless/lib/wireless.c	(revision 340)
@@ -1,3 +1,37 @@
+/**
+ * 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 wireless.c
+ * @brief Wireless Library Implementation
+ *
+ * Implementation of the wireless library.
+ *
+ * @author Brian Coltin, Colony Project, CMU Robotics Club
+ **/
+
 #include "wireless.h"
 #include "xbee.h"
 #include <stdlib.h>
@@ -6,7 +40,7 @@
 #include "wl_defs.h"
 
 #ifndef ROBOT
-#include <time.h>
+#include <sys/time.h>
 #include <signal.h>
 #else
 #include <time.h>
@@ -34,7 +68,6 @@
 PacketGroupHandler* wl_packet_groups[WL_MAX_PACKET_GROUPS];
 
 #ifndef ROBOT
-timer_t wl_timeout_timer;
 
 //called when we time out, or receive interrupt
 void sig_handler(int signo)
@@ -51,6 +84,14 @@
 	}
 	return;
 }
+#else
+
+//called when the timer ticks
+void timer_handler(void)
+{
+	wl_timeout = 1;
+}
+
 #endif
 
 /**
@@ -57,45 +98,52 @@
  * Initializes the wireless library. Must be called before any
  * other function.
  **/
-void wl_init()
+int wl_init()
 {
 	int i;
 	for (i = 0; i < WL_MAX_PACKET_GROUPS; i++)
 		wl_packet_groups[i] = NULL;
 
-	xbee_lib_init();
-	
+	if (xbee_lib_init() == -1) {
+	  return -1;
+	}
+
 	//begin timeout timer
 	#ifdef ROBOT
 	#ifdef FIREFLY
-	rtc_init(PRESCALE_DIV_128, 32, &wl_do_timeout);
+	rtc_init(PRESCALE_DIV_256, 32, &timer_handler);
 	#else
-	rtc_init(HALF_SECOND, &wl_do_timeout); 
+	rtc_init(HALF_SECOND, &timer_handler);
 	#endif
 	#else
-	//create a timer to trigger every half second
-	struct sigevent evp;
-	evp.sigev_signo = SIGALRM;
-	evp.sigev_notify = SIGEV_SIGNAL;
-	if (timer_create(CLOCK_REALTIME, &evp, &wl_timeout_timer) == -1)
-	{ 
-		WL_DEBUG_PRINT("Error creating a timer.\r\n"); 
-		exit(1); 
+
+	//create our timer
+	struct itimerval timer_val;
+	struct timeval interval;
+	interval.tv_sec = 0;
+	interval.tv_usec = 500000;
+	struct timeval first_time;
+	first_time.tv_sec = 0;
+	first_time.tv_usec = 500000;
+	timer_val.it_interval = interval;
+	timer_val.it_value = first_time;
+	if(setitimer(ITIMER_REAL,&timer_val,NULL)==-1)
+	{
+		WL_DEBUG_PRINT("Error creating a timer.\r\n");
+		perror("Failure's cause");
+		exit(1);
 	}
+
+	//create signal handler
 	struct sigaction wl_sig_act;
-	wl_sig_act.sa_handler = (void *)sig_handler;
+	wl_sig_act.sa_handler = sig_handler;
 	wl_sig_act.sa_flags = 0;
 	sigemptyset(&wl_sig_act.sa_mask);
 	sigaction(SIGALRM, &wl_sig_act, 0);
 	sigaction(SIGINT, &wl_sig_act, 0);
-	struct itimerspec wl_timeout_time;
-	wl_timeout_time.it_interval.tv_sec = 0;
-	wl_timeout_time.it_interval.tv_nsec = 500000000;
-	wl_timeout_time.it_value.tv_sec = 0;
-	wl_timeout_time.it_value.tv_nsec = 500000000;
-	timer_settime(wl_timeout_timer, 0,
-                        &wl_timeout_time, NULL);
 	#endif
+
+	return 0;
 }
 
 /**
@@ -103,16 +151,12 @@
  **/
 void wl_terminate()
 {
-	#ifndef ROBOT
-	timer_delete(wl_timeout_timer);
-	#endif
-	
 	int i;
 	for (i = 0; i < WL_MAX_PACKET_GROUPS; i++)
 		if (wl_packet_groups[i] != NULL &&
 			wl_packet_groups[i]->unregister != NULL)
 			wl_packet_groups[i]->unregister();
-	
+
 	xbee_terminate();
 }
 
@@ -169,7 +213,7 @@
  *
  * @return the 16-bit address of the XBee module.
  **/
-unsigned int wl_get_xbee_id()
+int wl_get_xbee_id()
 {
 	return xbee_get_address();
 }
@@ -236,7 +280,7 @@
 void wl_send_pan_packet(char group, char type,
 		char* data, int len, char frame)
 {
-	wl_send_packet(group, type, data, len, XBEE_BROADCAST, 
+	wl_send_packet(group, type, data, len, XBEE_BROADCAST,
 			XBEE_OPTIONS_NONE, frame);
 }
 
@@ -289,7 +333,7 @@
 
 /**
  * Unregister a packet group from the wireless library.
- * 
+ *
  * @param h the packet group to remove
  **/
 void wl_unregister_packet_group(PacketGroupHandler* h)
@@ -327,11 +371,11 @@
 		wl_do_timeout();
 		wl_timeout = 0;
 	}
-	
+
 	int len = xbee_get_packet(wl_buf);
 	if (len < 0)//no packet received
 		return;
-	
+
 	if (wl_buf[0] == XBEE_TX_STATUS)
 	{
 		if (len != 3)
@@ -339,7 +383,7 @@
 			WL_DEBUG_PRINT("Transmit Status packet should be of length 3.\r\n");
 			return;
 		}
-		
+
 		//the first four bits are the packet group
 		//this only works with under 16 groups
 		int group = (int)(wl_buf[1] >> 4);
@@ -350,11 +394,15 @@
 		{
 			WL_DEBUG_PRINT("No response received.\r\n");
 			if (wl_buf[2] == 2)
+			{
 				WL_DEBUG_PRINT("CCA Failure\r\n");
+			}
 			if (wl_buf[2] == 3)
+			{
 				WL_DEBUG_PRINT("Purged\r\n");
+			}
 		}
-		
+
 		if (wl_packet_groups[group] != NULL &&
 					wl_packet_groups[group]->handle_response != NULL)
 			wl_packet_groups[group]->handle_response(
@@ -361,7 +409,7 @@
 					(int)wl_buf[1] & 0x0F, success);
 		return;
 	}
-	
+
 	if (wl_buf[0] == XBEE_RX)
 	{
 		if (len < 7)
@@ -369,9 +417,9 @@
 			WL_DEBUG_PRINT("Packet is too small.\r\n");
 			return;
 		}
-		
+
 		int source = ((int)wl_buf[1] << 8) + ((int)wl_buf[2]);
-		
+
 		/*
 		//unused for now
 		int signalStrength = wl_buf[3];
@@ -378,19 +426,27 @@
 		//1 for Address broadcast, 2 for PAN broadcast
 		int options = wl_buf[4];
 		*/
-		
+
 		int group = wl_buf[5];
 		int type = wl_buf[6];
 		int packetLen = len - 7;
-		
+
 		if (wl_packet_groups[group] != NULL
 				&& wl_packet_groups[group]->handle_receive != NULL)
-			wl_packet_groups[group]->handle_receive(type, source, 
+			wl_packet_groups[group]->handle_receive(type, source,
 				wl_buf + 7, packetLen);
 		return;
 	}
-	
+
 	WL_DEBUG_PRINT("Unexpected packet received from XBee.\r\n");
 	return;
 }
 
+
+#ifndef ROBOT
+void wl_set_com_port(char* port)
+{
+	xbee_set_com_port(port);
+}
+#endif
+
