Index: branches/encoders/code/behaviors/spline/slave/slave.c
===================================================================
--- branches/encoders/code/behaviors/spline/slave/slave.c	(revision 1344)
+++ branches/encoders/code/behaviors/spline/slave/slave.c	(revision 1345)
@@ -18,9 +18,12 @@
 volatile int left_ref;
 volatile int right_ref;
 
-volatile int left_old;
-volatile int right_old;
+volatile int left_err_old;
+volatile int right_err_old;
 
+volatile int left_vel_old = 0;
+volatile int right_vel_old = 0;
+
 volatile int left_i = 0;
 volatile int right_i = 0;
 
@@ -27,19 +30,24 @@
 volatile int left_u;
 volatile int right_u;
 
-volatile int left_vel = 0;
-volatile int right_vel = 0;
+volatile int left_vel;
+volatile int right_vel;
 
 #define PL	1
 #define DL	0.1
 #define IL	0.1
 
+/*
 #define PR	1.5
 #define DR	0.2
 #define IR	0.1
+*/
+#define PR	1
+#define DR	0.1
+#define IR	0.1
 
 
-#define INTERVAL	50
+#define INTERVAL	20
 
 /* receive motor speed from master: 
  * left_speed left_dir right_speed right_dir
@@ -50,10 +58,14 @@
 	if (type != MOTOR) return;
 	
 	left_ref = packet[0];
+	if (left_ref > 127)
+		left_ref -= 256;
 	right_ref = packet[1];
+	if (right_ref > 127)
+		right_ref -= 256;
 
-	sprintf(buf, "received: %d %d\n", left_ref, right_ref);
-	usb_puts(buf);
+//	sprintf(buf, "received: %d %d\n", left_ref, right_ref);
+//	usb_puts(buf);
 	//orb_set_color(GREEN);
 }
 
@@ -61,17 +73,25 @@
 	int left_err = left_ref - left_vel;
 	int right_err = right_ref - right_vel;
 	
-	left_err = left_vel == 2048 ? left_old : left_err;
-	right_err = right_vel == 2048 ? right_old : right_err;
+	if (!left_ref)
+		left_err = 0;
+	if (!right_ref)
+		right_err = 0;
+	
+//	left_err = left_vel == 2048 ? left_err_old : left_err;
+//	right_err = right_vel == 2048 ? right_err_old : right_err;
 
 	left_i += left_err;
-	left_u = (int)(PL*(float)left_err + DL*(float)(left_err - left_old) + IL*(float)left_i);
-	left_old = left_err;
+	left_u = (int)(PL*(float)left_err + DL*(float)(left_err - left_err_old) + IL*(float)left_i);
+	left_err_old = left_err;
 	
 	right_i += right_err;
-	right_u = (int)(PR*(float)right_err + DR*(float)(right_err - right_old) + IR*(float)right_i);
-	right_old = right_err;
+	right_u = (int)(PR*(float)right_err + DR*(float)(right_err - right_err_old) + IR*(float)right_i);
+	right_err_old = right_err;
 
+//	sprintf(buf, "u: %d %d\n", left_u, right_u);
+//	usb_puts(buf);
+	
 	if (left_u < 0) {
 		left_dir = 0;
 		left_u = -left_u;
@@ -91,6 +111,11 @@
 
 	left_u = left_u > 210 ? 210 : left_u;
 	right_u = right_u > 210 ? 210 : right_u;
+
+	if (!left_ref)
+		left_u = 0;
+	if (!right_ref)
+		right_u = 0;
 }
 
 int main (void) {
@@ -101,8 +126,7 @@
 	//usb_puts("xbee id ");
 	//usb_putc(wl_get_xbee_id());
 	wl_init();
-    left_ref = 20;
-	right_ref = 20;
+
 	wl_register_packet_group(&packetHandler);
 	//orb_set_color(RED);
 	//motor1_set(FORWARD,230);
@@ -109,34 +133,42 @@
 	//motor2_set(FORWARD,230);
 	
 	while (1) {
-		usb_puts("We Win\n");
-	    //left_vel = encoder_get_v(0);
-		//right_vel = encoder_get_v(1);
-		 delay_ms(1000);
-		//if (!(count % INTERVAL)) {
-		//	wl_send_global_packet(GROUP, ENCODER, encoder, 2, 0);
+		
+		left_vel = encoder_get_v(0);
+		right_vel = encoder_get_v(1);
+		// err handling
+		if (left_vel == 2048)
+			left_vel = left_vel_old;
+		else 
+			left_vel_old = left_vel;
+		
+		if (right_vel == 2048)
+			right_vel = right_vel_old;
+		else 
+			right_vel_old = right_vel;
+		
+		
+		if (!(count % INTERVAL)) {
+			wl_send_global_packet(GROUP, ENCODER, encoder, 2, 0);
 			encoder[0] = left_vel;
 			encoder[1] = right_vel;
-		
-		   // usb_puti(left_vel);
-		  //  sprintf(buf, "sent (%d): %d %d %d %d\n", count, left_vel, right_vel, encoder[0], encoder[1]);
-		   // usb_puts(buf);
-		   // usb_puti(encoder[0]);
-	  //  }
-		
+			//sprintf(buf, "sent (%d): %d %d %d %d\n", count, left_vel, right_vel, encoder[0], encoder[1]);
+			//usb_puts(buf);
+		}	
 
-		//ctrl();
+		ctrl();
 	
-	  //  if (!(count % INTERVAL))
-	  //  	wl_do();
+		if (!(count % INTERVAL))
+			wl_do();
 		
 //		sprintf(buf, "motor (%d): %d %d %d %d\n", count, left_dir, left_ref, right_dir, right_ref);
 //		usb_puts(buf);
 
-	    //motor1_set(left_dir, left_u);
-		//motor2_set(right_dir, right_u);
+		motor1_set(left_dir, left_u);
+		motor2_set(right_dir, right_u);
 		
 		count++;
-	    delay_ms(1000);
+//		delay_ms(100);
 	}
 }
+
Index: branches/encoders/code/behaviors/spline/slave/Makefile
===================================================================
--- branches/encoders/code/behaviors/spline/slave/Makefile	(revision 1344)
+++ branches/encoders/code/behaviors/spline/slave/Makefile	(revision 1345)
@@ -92,7 +92,7 @@
 # Optimization level, can be [0, 1, 2, 3, s].
 #     0 = turn off optimization. s = optimize for size.
 #     (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
-OPT = s
+OPT = 0
 
 # Debugging format.
 #     Native formats for AVR-GCC's -g are dwarf-2 [default] or stabs.
Index: branches/encoders/code/behaviors/spline/master/master.c
===================================================================
--- branches/encoders/code/behaviors/spline/master/master.c	(revision 1344)
+++ branches/encoders/code/behaviors/spline/master/master.c	(revision 1345)
@@ -23,7 +23,6 @@
 PacketGroupHandler packetHandler = {GROUP, 0, 0, &packet_receive, 0};
 
 void packet_receive (char type, int source, char* packet, int length) {
-
 	if(type == ENCODER)
 	{ 
 		if(id == -1)
@@ -32,10 +31,7 @@
 			motor1_set(1, 200);
 		usb_putc(packet[0]);
 		usb_putc(packet[1]);
-//		usb_putc(packet[2]);
-//		usb_putc(packet[3]);
 	}                                  
-	
 }
 
 int main (void) {
@@ -55,9 +51,7 @@
 		lcd_puts("starting init");
 		data[0] = usb_getc(); 
 		data[1] = usb_getc();
-		//data[2] = usb_getc();
-		//data[3] = usb_getc();
-		
+
 		lcd_puts("Done with init\n");
 
 		wl_send_global_packet(GROUP, MOTOR, data, 2, 0);
@@ -67,19 +61,16 @@
 		wl_register_packet_group(&packetHandler);
 	}
 
-//	lcd_clear_screen();
-  while (1) {
-  if(position == SENDER)
-  {
-		data[0] = usb_getc(); 
-		data[1] = usb_getc();
-		//data[2] = usb_getc();
-		//data[3] = usb_getc();
-		wl_send_global_packet(GROUP, MOTOR, data, 2, 0);
-	}	
+	while (1) {
+		if(position == SENDER)
+		{
+			data[0] = usb_getc(); 
+			data[1] = usb_getc();
+			wl_send_global_packet(GROUP, MOTOR, data, 2, 0);
+		}        
 
-	  wl_do();
-  }
-  
+		wl_do();
+	}
+
 }
-                                                   
+
Index: branches/encoders/code/lib/include/libwireless/sensor_matrix.h
===================================================================
--- branches/encoders/code/lib/include/libwireless/sensor_matrix.h	(revision 1344)
+++ branches/encoders/code/lib/include/libwireless/sensor_matrix.h	(revision 1345)
@@ -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
@@ -17,53 +46,56 @@
  * @{
  **/
 
+#define MAXIMUM_XBEE_ID		0x10
+#define READING_UNKNOWN		0xFF
+
 /**
  * @struct SensorMatrix
  *
  * A sensor matrix.
  **/
+//TODO: the order of member variables in this struct should be changed in case the compile packs the struct
+// In order to achieve the best packing, the variables should be listed in order of decreasing memory size.
+// Thus, pointers should be first, followed by int, followed by char.
 typedef struct
 {
 	/**
-	 * The size of the sensor matrix.
-	**/
-	unsigned int size;
-	/**
-	 * The matrix. Each row represents the readings of one
-	 * robot.
+	 * The number of robots in the token ring.
 	 **/
-	int** matrix;
+	int numJoined;
 	/**
 	 * The element representing a robot is true if that robot
 	 * is in the token ring and false otherwise.
 	 **/
-	int* joined;
+	unsigned char joined[MAXIMUM_XBEE_ID];
+
+	// on the bayboard, we don't include the matrix to save memory.
+#ifndef BAYBOARD
 	/**
-	 * The number of robots in the token ring.
+	 * The matrix. Each row represents the readings of one
+	 * robot.
 	 **/
-	int numJoined;
+	unsigned char matrix[MAXIMUM_XBEE_ID][MAXIMUM_XBEE_ID];
+#endif
 } SensorMatrix;
 
 /**@brief Create a sensor matrix **/
-SensorMatrix* sensor_matrix_create(void);
-/**@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);
-/**@brief Remove a robot from a sensor matrix **/
-void sensor_matrix_remove_robot(SensorMatrix* m, unsigned int id);
+void sensor_matrix_create(void);
 /**@brief Set a reading in a sensor matrix **/
-void sensor_matrix_set_reading(SensorMatrix* m, int observer, int robot, int reading);
+void sensor_matrix_set_reading(int observer, int robot, int reading);
 /**@brief Get a reading in a sensor matrix **/
-int sensor_matrix_get_reading(SensorMatrix* m, int observer, int robot);
+int sensor_matrix_get_reading(int observer, int robot);
 /**@brief Set whether the robot is in the token ring **/
-void sensor_matrix_set_in_ring(SensorMatrix* m, int robot, int in);
+void sensor_matrix_set_in_ring(int robot, int in);
 /**@brief Get whether the robot is in the sensor ring **/
-int sensor_matrix_get_in_ring(SensorMatrix* m, int robot);
-/**@brief Get the size of the sensor matrix **/
-int sensor_matrix_get_size(SensorMatrix* m);
+int sensor_matrix_get_in_ring(int robot);
 /**@brief Get the number of robots which have joined the token ring **/
-int sensor_matrix_get_joined(SensorMatrix* m);
+int sensor_matrix_get_joined(void);
+/**@brief Get the maximum size of the sensor matrix **/
+int sensor_matrix_get_size(void);
 
 /** @} **/ //end defgroup
 
+
+#endif
+
Index: branches/encoders/code/lib/include/libwireless/wireless.h
===================================================================
--- branches/encoders/code/lib/include/libwireless/wireless.h	(revision 1344)
+++ branches/encoders/code/lib/include/libwireless/wireless.h	(revision 1345)
@@ -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,12 +31,19 @@
  *
  * @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.
 /**
  * The maximum number of packet groups.
  **/
+//TODO: a PacketGroupHandler is at least 10 bytes (I don't know if function pointers are 2 bytes
+// or 4 bytes).  That means that in the c file, your array of packet groups is at least 160 bytes.
+// Normally that might be fine (the robot's avr chips have 4k SRAM), but austin's chip only has
+// 1k SRAM, so if this number can be reduced or if the size of the struct could be reduced, that would be a plus.
 #define WL_MAX_PACKET_GROUPS 16
 
 /**
@@ -20,12 +52,12 @@
  *
  * Contains functions and definitions for dealing with wireless functionality.<br><br>
  *
- * The wireless library provides a modular method for dealing with 
+ * The wireless library provides a modular method for dealing with
  * wireless packets, by allowing packet groups to be registered.
  * A packet group is a collection of packets which share a packet
- * group code. Each packet in the group also has a type. A packet 
+ * group code. Each packet in the group also has a type. A packet
  * group code and type are sent with each packet. When a packet
- * with a group code registered in the wireless library is 
+ * with a group code registered in the wireless library is
  * received, the corresponding event handler is called. The
  * event handler uses the packet type and other information
  * stored in the packet to respond.<br><br>
@@ -39,11 +71,14 @@
 
 /**
  * @struct PacketGroupHandler
- * A PacketGroupHandler represents a packet group, and is used to 
+ * A PacketGroupHandler represents a packet group, and is used to
  * register a packet group with the wireless library. It contains
  * handlers for various events which can occur related to a packet
  * group.
  **/
+//TODO: the order of member variables in this struct should be changed in case the compile packs the struct
+// In order to achieve the best packing, the variables should be listed in order of decreasing memory size.
+// Thus, pointers should be first, followed by int, followed by char.
 typedef struct
 {
 	/**
@@ -51,6 +86,8 @@
 	 * must be unique. The maximum number of packet groups
 	 * is defined by WL_MAX_PACKET_GROUPS.
 	 **/
+  //TODO: if this number must be less than or equal to WL_MAX_PACKET_GROUPS, don't you only need
+  // one byte for it and it can be made an unsigned char?
 	unsigned int groupCode;
 
 	/**
@@ -58,7 +95,7 @@
 	 * but in wl_do).
 	 **/
 	void (*timeout_handler) (void);
-	
+
 	/**
 	 * Called when a transmit status packet is received
 	 * from the XBee where the first four bits of the frame
@@ -69,7 +106,7 @@
 	 * we did not.
 	 **/
 	void (*handle_response) (int frame, int received);
-	
+
 	/**
 	 * Called when we receive a packet from this group.
 	 *
@@ -79,18 +116,17 @@
 	 * @param packet the packet received
 	 * @param length the length of the packet
 	 **/
-	void (*handle_receive) (char type, int source, unsigned char* packet,
-							int length);
-	
+	void (*handle_receive) (char type, int source, unsigned char* packet, int length);
+
 	/**
 	 * Called for any cleanup when the network is turned off.
 	 **/
 	void (*unregister) (void);
-	
+
 } 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 **/
@@ -101,28 +137,28 @@
 void wl_unregister_packet_group(PacketGroupHandler* h);
 
 /**@brief Send a packet to a specific robot in any PAN **/
-void wl_send_robot_to_robot_global_packet(char group, char type,
-		char* data, int len, int dest, char frame);
+int wl_send_robot_to_robot_global_packet(char group, char type, char* data, int len, int dest, char frame);
 /**@brief Send a packet to a specific robot in our PAN **/
-void wl_send_robot_to_robot_packet(char group, char type,
-		char* data, int len, int dest, char frame);
+int wl_send_robot_to_robot_packet(char group, char type, char* data, int len, int dest, char frame);
 /**@brief Send a packet to all robots **/
-void wl_send_global_packet(char group, char type,
-		char* data, int len, char frame);
+int wl_send_global_packet(char group, char type, char* data, int len, char frame);
 /**@brief Send a packet to all robots in our PAN **/
-void wl_send_pan_packet(char group, char type,
-		char* data, int len, char frame);
+void wl_send_pan_packet(char group, char type, char* data, int len, char frame);
 
 /**@brief Set the PAN we are using **/
-void wl_set_pan(int pan);
+int wl_set_pan(int pan);
 /**@brief Get the PAN we are using **/
 int wl_get_pan(void);
 /**@brief Set the channel we are using **/
-void wl_set_channel(int channel);
+int wl_set_channel(int channel);
 /**@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/encoders/code/lib/include/libwireless/wl_token_ring.h
===================================================================
--- branches/encoders/code/lib/include/libwireless/wl_token_ring.h	(revision 1344)
+++ branches/encoders/code/lib/include/libwireless/wl_token_ring.h	(revision 1345)
@@ -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
@@ -19,19 +47,17 @@
  **/
 
 /**@brief Register the token ring group with the wireless library.**/
-void wl_token_ring_register(void);
+int wl_token_ring_register(void);
 /**@brief Unregister the token ring group with the wirelss library.**/
 void wl_token_ring_unregister(void);
 /**@brief Set the functions called to turn the bom on and off.**/
-void wl_token_ring_set_bom_functions(void (*on_function) (void), 
-		void (*off_function) (void), int (*max_bom_function) (void));
+void wl_token_ring_set_bom_functions(void (*on_function) (void), void (*off_function) (void),
+  int (*max_bom_function) (void));
 
 /**@brief Join the token ring **/
-void wl_token_ring_join(void);
+int 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/encoders/code/lib/include/libwireless/wl_defs.h
===================================================================
--- branches/encoders/code/lib/include/libwireless/wl_defs.h	(revision 1344)
+++ branches/encoders/code/lib/include/libwireless/wl_defs.h	(revision 1345)
@@ -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
 
@@ -15,29 +53,21 @@
 #define WL_TOKEN_RING_GROUP 2
 
 #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_SENSOR_MATRIX 2
+#define WL_TOKEN_BOM_ON 3
+#define WL_TOKEN_JOIN 4
+#define WL_TOKEN_JOIN_ACCEPT 5
 
 // 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
-#define WL_RECHARGE_GROUP 3
-
-#define WL_RECHARGE_POLL_STATIONS 1
-#define WL_RECHARGE_STATION_AVAILABLE 2
-#define WL_RECHARGE_REQUEST 3
-#define WL_RECHARGE_REQUEST_ACCEPT 4
-#define WL_RECHARGE_VERIFY 5
-#define WL_RECHARGE_CANCEL 6
-#define WL_RECHARGE_SEEKING 7
-#define WL_RECHARGE_DOCKED 8
-
 #ifdef WL_DEBUG
 
 #ifdef ROBOT
@@ -63,3 +93,5 @@
 
 #endif
 
+#endif
+
Index: branches/encoders/code/lib/include/libwireless/xbee.h
===================================================================
--- branches/encoders/code/lib/include/libwireless/xbee.h	(revision 1344)
+++ branches/encoders/code/lib/include/libwireless/xbee.h	(revision 1345)
@@ -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,24 +74,26 @@
 #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 **/
 int xbee_get_packet(unsigned char* packet);
 /**@brief Send a packet to the XBee **/
-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);
 /**@brief Set the PAN ID for the XBee **/
-void xbee_set_pan_id(int id);
+int xbee_set_pan_id(int id);
 /**@brief Get the XBee's PAN ID **/
 unsigned int xbee_get_pan_id(void);
 /**@brief Set the channel the XBee is currently using **/
-void xbee_set_channel(int channel);
+int xbee_set_channel(int channel);
 /**@brief Get the channel the XBee is currently using **/
 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/encoders/code/lib/include/libwireless/wl_error_group.h
===================================================================
--- branches/encoders/code/lib/include/libwireless/wl_error_group.h	(revision 1344)
+++ branches/encoders/code/lib/include/libwireless/wl_error_group.h	(revision 1345)
@@ -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/encoders/code/lib/include/libdragonfly/lights.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/lights.h	(revision 1344)
+++ branches/encoders/code/lib/include/libdragonfly/lights.h	(revision 1345)
@@ -1,4 +1,34 @@
+// FIXME remove
+typedef unsigned char uint8_t;
+
+
 /**
+ * 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 lights.h
  * @brief Contains declarations for managing the orbs.
  *
@@ -11,13 +41,82 @@
 #ifndef _LIGHTS_H_
 #define _LIGHTS_H_
 
+/**
+ * @addtogroup orbs
+ * @{
+ **/
 
 /**
+ * Quick start: call orb_init_pwm or orb_init_binary, depending on which mode you want to use. Call orb*set or
+ * orb*set_color to set the orbs.
+ * 
+ * The orbs have two modes of operation: PWM mode and binary mode. In PWM mode, a pwm signal is generated by a hardware
+ * timer and the orbs can be set to a value of 0 through 255. In binary mode, the orbs can only be turned on or off and
+ * a value of 0 means "off" and any other value means "on". The mode can be chosen on initialization and can be changed
+ * at runtime using the orb_set_mode function.
+ *
+ * Operation (PWM mode): On timer overflow, all LEDs with a value>0 are turned on and the output compare value for the
+ * first LED is loaded. On compare match, the corresponding LED is turned off and the next output compare value is
+ * loaded. All masks are precomputed and sorted by time when setting the values.
+ *
+ * The data structure (pwm_t) containing the PWM times and masks is triple buffered. This is because the buffer the ISR
+ * is reading from may only be modified on timer overflow before the next PWM sequence is started, because otherwise the
+ * next OCR value might be sed to a value smaller than the current timer value, resulting in the remaining channels not
+ * being turned off in that PWM period (flash to on). When using two buffers, the page flip can only occur on a timer
+ * overflow for the same reason. So after writing a buffer and marking it for page flip, neither of the buffers could be
+ * modified because the front buffer is read by the ISR and the back buffer could be switched at any time. So the
+ * calling thread would have to be delayed by up to one full PWM period (8ms in the current implementation, but
+ * 20ms-50ms would be a reasonable value to expect here). To avoid this, triple buffering is used.
+ *
+ * The code for applying the orbs is fairly optimized. See the apply_orbs function for some time measurements and
+ * further nodes.
+ *
+ * The PWM frequency is 120Hz (8ms period time). The next lower frequency (determined by the prescaler) is 30 Hz which
+ * is too slow (orbs flicker).
+ *
+ * The orbs code is thread safe, which means that the functions may be called from another interrupt handler. If there
+ * are multiple concurrent calls to the orb*set* functions, one of them is ignored and the orbs are never left in an
+ * inconsistent state. For example, if the orbs are set to green by the main thread and to red by an interrupt handler,
+ * the resulting color will be either red or green, but never yellow.
+ * Thread safety is achieved by grabbing a lock at the beginning of all functions that modify the orb code and releasing
+ * the lock at the end. If the lock is already taken, the function just returns doing nothing.
+ *
+ * Some performance measurements:
+ *   - Time for setting new orb values (PWM mode):       35us-72us (depending on the degree to which the array is
+ *                                                                  already in the correct order)
+ *   - Time for setting new orb values (binary mode):        5.5us
+ *
+ *   - Interrupt time (PWM mode only):                         8us (overflow)
+ *                                                            10us (output compare)
+ *                                                             6us (last output compare)
+ *                                                            30us (output compare, all value equal)
+ *
+ *   - Maximum total interrupt time per period:               64us
+ *   - Maximum CPU usage for interrupts (PWM mode only):     <0.8%
+ *
+ *   - Maximum contiguous synchronized block:                 30us (output compare interrupt, all values equal)
+ *
+ * There are some potential optimizations left. See the source code for more information.
+ *
+ * A note on robustness: if the output compare interrupt is disabled for too long, either due to a long ISR or a long
+ * synchronized code block, the orbs will flicker to brighter values for being turned off too late. With software PWM,
+ * there's nothing at all to be done about that. The problem can be alleviated by using a lower PWM frequency, but then
+ * the orbs will start flickering all the time due to the low update frequency.
+ * Some measurements: with 100us synchronized blocks, the flickering is accepptably low. Longer synchronized blocks
+ * mean more flickering. At 1ms synchronized blocks, the flickering is quite bad, especially for low orb values. Note
+ * that orb value 0 never flickers at all because the corresponding channels are not turned on at all.
+ * Test code (note the _delay_us restrictions!)
+ *   orb_set (1,1,1); while (1) { SYNC { for (uint8_t m=0; m<10; ++m) { _delay_us(10); } } }
+ **/
+
+/** @} **/ //end addtogroup
+
+/**
  * @addtogroup orbs
  * @{
  **/
 
-//ORB Colors
+// ***** Predefined colors *****
 /** @brief Red **/
 #define RED       0xE0
 /** @brief Orange **/
@@ -40,34 +139,84 @@
 #define MAGENTA   0xE3
 /** @brief White **/
 #define WHITE     0xFE
-/** @brief Turn the orb off (White) **/
-#define ORB_OFF   0xFE      //ORB_OFF->WHITE
+/** @brief Turn the orb off **/
+#define ORB_OFF   0x00
 
-/** @brief Enables the orbs **/
+
+
+// ***** Initialization *****
+
+/** @brief Enables the orbs in default mode **/
 void orb_init(void);
+
+/** @brief Enables the orbs in binary mode **/
+void orb_init_binary (void);
+
+/** @brief Enables the orbs in PWM mode **/
+void orb_init_pwm (void);
+
+
+
+// ***** Mode setting *****
+
+/** Specification of the orb mode **/
+typedef uint8_t orb_mode_t;
+
+/** @brief PWM mode **/
+#define orb_mode_pwm 0
+
+/** @brief Binary mode **/
+#define orb_mode_binary 1
+
+
+/** @brief Switches the orbs to the specified mode **/
+void orb_set_mode (orb_mode_t mode);
+
+/** @brief Disables the orb timer, but does not change the mode **/
+void orb_disable_timer (void);
+
+/** @brief Enables the orb timer, but does not change the mode **/
+void orb_enable_timer (void);
+
+
+
+// ***** Setting RGB colors *****
+
+/** @brief set the specified orb to a specified color **/
+void orb_n_set (uint8_t num, uint8_t red, uint8_t green, uint8_t blue);
+
 /** @brief Set both orbs to a specified color **/
-void orb_set(unsigned char red_led, unsigned char green_led,
-	unsigned char blue_led);
+void orb_set(uint8_t red, uint8_t green, uint8_t blue);
+
 /** @brief Set orb1 to a specified color **/
-void orb1_set(unsigned char red_led, unsigned char green_led,
-	unsigned char blue_led); 
+void orb1_set(uint8_t red_led, uint8_t green_led, uint8_t blue_led); 
+
 /** @brief Set orb2 to a specified color **/
-void orb2_set(unsigned char red_led, unsigned char green_led,
-	unsigned char blue_led);
+void orb2_set(uint8_t red_led, uint8_t green_led, uint8_t blue_led);
 
-/** @brief Set both orbs to a specified color **/
-void orb_set_color(int col);
+void orbs_set (uint8_t red1, uint8_t green1, uint8_t blue1, uint8_t red2, uint8_t green2, uint8_t blue2);
+
+
+
+// ***** Settings predefined colors *****
+
+/** @brief set the specified orb to the specified color **/
+void orb_n_set_color(uint8_t num, uint8_t col);
+
 /** @brief Set orb1 to a specified color **/
-void orb1_set_color(int col);
+void orb1_set_color(uint8_t col);
+
 /** @brief Set orb2 to a specified color **/
-void orb2_set_color(int col);
+void orb2_set_color(uint8_t col);
 
-/** @brief Disable the orbs **/
-void orb_disable(void);
-/** @brief Enable the orbs **/
-void orb_enable(void);
+/** @brief set the orbs to specified colors **/
+void orbs_set_color(uint8_t col1, uint8_t col2);
 
+/** @brief Set both orbs to a specified color **/
+void orb_set_color(uint8_t col);
+
+
+
 /** @} **/ //end addtogroup
 
 #endif
-
Index: branches/encoders/code/lib/include/libdragonfly/spi.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/spi.h	(revision 1344)
+++ branches/encoders/code/lib/include/libdragonfly/spi.h	(revision 1345)
@@ -4,6 +4,11 @@
  * @author Colony Project, CMU Robotics Club
  **/
 
+/**
+ * @addtogroup spi
+ * @{
+ **/
+
 #ifndef __SPI_H__
 #define __SPI_H__
 
@@ -21,7 +26,21 @@
 typedef void (*spi_fun_recv_t)(char);
 typedef void (*spi_fun_recv_complete_t)(void);
 
+/** 
+* @brief Initialize SPI
+* 
+* @param spi_fun_recv_t The function that handles SPI data, byte for byte.
+* @param spi_fun_recv_complete_t  Called on a completed transmission - typically for cleaning up.
+*/
 void spi_init (spi_fun_recv_t, spi_fun_recv_complete_t);
+
+/** 
+* @brief Initialize SPI transfer.
+* 
+* @param char The number of bytes to transfer.
+*/
 void spi_transfer (char);
 
+/**@}**/ //end group
+
 #endif
Index: branches/encoders/code/lib/include/libdragonfly/time.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/time.h	(revision 1344)
+++ branches/encoders/code/lib/include/libdragonfly/time.h	(revision 1345)
@@ -1,4 +1,30 @@
 /**
+ * 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 time.h
  * @brief Contains time-related functions and definitions
  *
@@ -11,10 +37,6 @@
 #ifndef _TIME_H_
 #define _TIME_H_
 
-/*Maximum # of functions*/
-#define MAX_FUNCTIONS 10
-
-
 /*	Predefined times for prescale_opt in time.c.
 	To make you own, know that a pulse is 1/16th of a second. You cannot get less than this. To get more, you need
 	to know how many 16ths of a second are in the time you want. (Time_desired * 16 = prescaler_opt)
@@ -41,19 +63,12 @@
 /** @brief Delay execution for the specified time **/
 void delay_ms(int ms) ;
 /** @brief Enable the realtime clock **/
-int rtc_multi_init(int* prescale_opt, void (*rtc_fun[])(void), int argc);
+void rtc_init(int prescale_opt, void (*rtc_func)(void));
 /** @brief Reset the counter of the realtime clock **/
 void rtc_reset(void);
 /** @brief Get the value of the realtime clock. **/
 int rtc_get(void);
 
-int rtc_add_function(void (*rtc_f) (void), int prescale_opt);
-
-int rtc_remove_function(void (*rtc_func)(void));
-int rtc_get_function_count(void);
-
-int rtc_init(int prescale_opt, void (*rtc_func)(void));
-
 /** @} **/
 
 #endif
Index: branches/encoders/code/lib/include/libdragonfly/motor.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/motor.h	(revision 1344)
+++ branches/encoders/code/lib/include/libdragonfly/motor.h	(revision 1345)
@@ -1,4 +1,30 @@
 /**
+ * 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 motor.h
  * @brief Contains definitions for controlling the motors
  *
Index: branches/encoders/code/lib/include/libdragonfly/analog.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/analog.h	(revision 1344)
+++ branches/encoders/code/lib/include/libdragonfly/analog.h	(revision 1345)
@@ -1,5 +1,30 @@
 /**
+ * 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 analog.h
  * @brief Contains functions and definitions for using the ADC
  * 
@@ -12,11 +37,13 @@
 	
  * @author Colony Project, CMU Robotics Club, based on firefly
  * code by Tom Lauwers
-*/
+ */
 
 #ifndef _ANALOG_H
 #define _ANALOG_H
 
+#include <inttypes.h>
+
 /**
  * @addtogroup analog
  * @{
@@ -43,18 +70,20 @@
 /** @brief Analog port 9 **/
 #define AN9 0x09
 /** @brief Analog port 10 **/
-#define AN10 0x0a
+#define AN10 0x0A
 /** @brief Analog port 11 **/
-#define AN11 0x0b
+#define AN11 0x0B
 /** @brief Analog port 12 **/
-#define AN12 0x0c
+#define AN12 0x0C
 /** @brief Analog port 13 **/
-#define AN13 0x0d
+#define AN13 0x0D
 /** @brief Analog port 14 **/
-#define AN14 0x0e
+#define AN14 0x0E
 /** @brief Analog port 15 **/
-#define AN15 0x0f
+#define AN15 0x0F
 
+/** @brief BOM_PORT analog port for BOM **/
+#define BOM_PORT AN0
 /** @brief EXT_MUX analog port **/
 #define EXT_MUX AN7
 /** @brief Analog port for the wheel **/
@@ -62,15 +91,38 @@
 /** @brief Analog port for the battery voltage detector **/
 #define BATT_PORT  AN11
 
-/** @brief Initialize analog ports. **/
-void analog_init(void);
-/** @brief Read an 8-bit number from an analog port. **/
+#define ADC_START 1
+#define ADC_STOP 0
+
+#define ADMUX_OPT 0x60
+
+/** @brief Struct to hold the value of a particular analog port **/
+typedef struct {
+  uint8_t adc8;
+  uint16_t adc10;
+} adc_t;
+
+
+/** @brief Initialize analog ports. Will start running a loop
+    if start_conversion is ADC_START.**/
+void analog_init(int start_conversion);
+/** @brief starts the analog loop. Doesn't do anything if the loop is already running. **/
+void analog_start_loop(void);
+/** @brief Stops the analog loop. Doesn't do anything if the loop is already stopped. **/
+void analog_stop_loop(void);
+/** @brief Returns an 8-bit analog value from the look up table. Use this instead of analog_get8. **/
 unsigned int analog8(int which);
-/** @brief Read a 10-bit number from an analog port. **/
+/** @brief Returns an 10-bit analog value from the look up table. Use this instead of analog_get10. **/
 unsigned int analog10(int which);
 /** @brief Read the position of the wheel. **/
 int wheel(void);
+/** @brief Read an 8-bit number from an analog port. Loop must be stopped for this to work. **/
+unsigned int analog_get8(int which);
+/** @brief Read a 10-bit number from an analog port. Loop must be stopped for this to work. **/
+unsigned int analog_get10(int which);
 
+
 /**@}**/ //end group
 
 #endif
+
Index: branches/encoders/code/lib/include/libdragonfly/odometry.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/odometry.h	(nonexistent)
+++ branches/encoders/code/lib/include/libdragonfly/odometry.h	(revision 1345)
@@ -0,0 +1,56 @@
+
+/**
+ * @file odometry.h
+ * @brief Code for estimating the robots pose.
+ * 
+ * Offers simple position and orientation information.
+ *
+ * @author Colony Project, CMU Robotics Club
+ **/
+
+#ifndef __ODOMETRY_C__
+#define __ODOMETRY_C__
+
+/**
+ * @addtogroup odometry 
+ * @{
+ **/
+
+//Odometry resolution, *64 microseconds.
+#define ODOMETRY_CLK 255u 
+#define TIME_SCALE 64
+
+//Wheel = 2.613 in.  
+//Circumference = 208.508133 mm
+//Distance per encoder click (circumference / 1024)  = 203.621224 um.
+//Robot width = 5.3745 in. = 136.5123 mm
+
+#define ROBOT_WIDTH_UM 137000  //um
+#define CLICK_DISTANCE_UM 204 //um
+
+#define DISTANCE_SCALE 2.10526316 //Magic constant.
+#define ANGLE_SCALE 1.12823207 //Magic constant.
+
+/** @brief Retrieve the robots estimated x position*/
+long odometry_dx(void);
+
+/** @brief Retrieve the robots estimated y position*/
+long odometry_dy(void);
+
+/** @brief Retrieve the robots estimated orientation*/
+double odometry_angle(void);
+
+/** @brief Initialize odometry. MUST be called before 
+ * the other functions work.**/
+void odometry_init(void);
+
+/** @brief Reset position and orientation to the origin facing
+ * the x axis.*/
+void odometry_reset(void);
+
+/** @brief Report estimated velocity [mm/s].*/
+long odometry_velocity(void);
+
+/**@}**/ //end group
+
+#endif
Index: branches/encoders/code/lib/include/libdragonfly/encoders.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/encoders.h	(nonexistent)
+++ branches/encoders/code/lib/include/libdragonfly/encoders.h	(revision 1345)
@@ -0,0 +1,80 @@
+/**
+ * 
+ * @file encoders.h
+ * @brief Contains functions for reading encoder values.
+ *
+ * Contains high and low level functions for reading encoders
+ * including reading out total distance covered, and 
+ * eventually velocity.
+ *	
+ * @author Colony Project, CMU Robotics Club
+*/
+
+/**
+ * @addtogroup encoders
+ * @{
+ **/
+
+#ifndef __ENCODERS_H__
+#define __ENCODERS_H__
+
+
+#ifndef LEFT
+	#define LEFT 0
+#endif
+#ifndef RIGHT
+	#define RIGHT 1
+#endif
+
+/** @brief Magnet misaligned - likely distance from encoder problem. **/
+#define ENCODER_MAGNET_FAILURE 1025
+/** @brief Encoder misaligned - likely on XY plane. **/
+#define ENCODER_MISALIGNED 1027
+/** @brief Not enough time has passed - encoders not initialized in hardware. **/
+#define ENCODER_DATA_NOT_READY 1026
+
+//delay_ms argument after a full read is complete
+#define ENCODER_DELAY 20
+
+//Data invalid flags (hardware failure):
+#define OCF _BV(4)
+#define COF _BV(3)
+
+//Data invalid alarm (May be invalid):
+#define LIN _BV(2)
+
+#define MagINCn _BV(1)
+#define MagDECn _BV(0)
+
+//////////////////////////////
+//#define BUFFER_SIZE 23
+#define BUFFER_SIZE		46
+#define ERR_VEL			1024
+//////////////////////////////
+
+
+/** @brief Initialize encoders. **/
+void encoders_init(void);
+/** @brief Read instantaneous encoder value. **/
+int encoder_read(char encoder);
+/** @brief Currently a stub - DO NOT Use. **/
+char encoder_direction(char encoder);
+
+/** @brief Get total distance traveled. **/
+int encoder_get_dx(char encoder);
+/** @brief Reset distance counter. **/
+void encoder_rst_dx(char encoder);
+/** @brief Get time count: The number of encoder reads that have occurred. **/
+int encoder_get_tc(void);
+/** @brief Reset the time count. **/
+void encoder_rst_tc(void);
+
+/** @brief Still untested, so use at your own risk. **/
+int encoder_get_v(char encoder);
+
+/** @brief Waits for the next encoder reading, then returns. **/
+void encoder_wait( int nReadings );
+
+/**@}**/ //end group
+
+#endif
Index: branches/encoders/code/lib/include/libdragonfly/lcd.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/lcd.h	(revision 1344)
+++ branches/encoders/code/lib/include/libdragonfly/lcd.h	(revision 1345)
@@ -1,4 +1,30 @@
 /**
+ * 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 lcd.h
  * @brief Contains definitions for dealing with the LCD screen.
  * 
@@ -32,3 +58,4 @@
 /** @} **/
 
 #endif
+
Index: branches/encoders/code/lib/include/libdragonfly/move.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/move.h	(revision 1344)
+++ branches/encoders/code/lib/include/libdragonfly/move.h	(revision 1345)
@@ -1,4 +1,30 @@
 /**
+ * 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 move.h
  * @brief Contains definitions for controlling robot motion
  * 
Index: branches/encoders/code/lib/include/libdragonfly/battery.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/battery.h	(revision 1344)
+++ branches/encoders/code/lib/include/libdragonfly/battery.h	(revision 1345)
@@ -1,4 +1,30 @@
 /**
+ * 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 battery.h
  * @brief Definitions for checking battery voltage.
  * 
@@ -29,7 +55,7 @@
 int battery(void);
 /** @brief Check if the battery is low. **/
 char battery_low(void);
-/** @brief get an average battery voltage reading. **/
+/** @brief Get an average battery voltage reading. **/
 int battery8_avg(int n_samples);
 
 /** @} **/ //end addtogroup
Index: branches/encoders/code/lib/include/libdragonfly/dio.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/dio.h	(revision 1344)
+++ branches/encoders/code/lib/include/libdragonfly/dio.h	(revision 1345)
@@ -1,4 +1,30 @@
 /**
+ * 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 dio.h
  * @brief Definitions for digital input / output
  *
@@ -13,17 +39,17 @@
 #define _DIO_H
 
 /*
-these are ALL the pins
-don't use unless you know what you're doing
+  these are ALL the pins
+  don't use unless you know what you're doing
 */
 
 /*
-DIO pins on new dragonfly boards are shown below:
------------------------------------------------
-| _PIN_E6 | _PIN_E7 | _PIN_D2 | _PIN_D3 | VCC |
------------------------------------------------
-| _PIN_E2 | _PIN_E3 | _PIN_E4 | _PIN_E5 | GND |
------------------------------------------------
+  DIO pins on new dragonfly boards are shown below:
+  -----------------------------------------------
+  | _PIN_E6 | _PIN_E7 | _PIN_D2 | _PIN_D3 | VCC |
+  -----------------------------------------------
+  | _PIN_E2 | _PIN_E3 | _PIN_E4 | _PIN_E5 | GND |
+  -----------------------------------------------
 */
 
 /** @brief Port A **/
@@ -168,8 +194,8 @@
 //#define _PIN_G7 63
 
 /*
-These are the header pins (the ones you can connect things to)
-Feel free to use these
+  These are the header pins (the ones you can connect things to)
+  Feel free to use these
 */
 
 /**
Index: branches/encoders/code/lib/include/libdragonfly/dragonfly_lib.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/dragonfly_lib.h	(revision 1344)
+++ branches/encoders/code/lib/include/libdragonfly/dragonfly_lib.h	(revision 1345)
@@ -1,74 +1,115 @@
-/**
- * @file dragonfly_lib.h
- * @brief Contains other include files
- * 
- * Include this file for all the functionality of libdragonfly.
- *
- * @author Colony Project, CMU Robotics Club
- **/
-
-#ifndef _DRAGONFLY_LIB_H_
-#define _DRAGONFLY_LIB_H_
-
-/**
- * @addtogroup dragonfly
- * @{
- **/
-
-// Configuration definitions
-/** @brief Initialize analog **/
-#define ANALOG 0x01
-/** @brief Initialize serial communications **/
-#define SERIAL 0x02
-/** @brief Initialize USB communications **/
-#define USB    0x02
-/** @brief Initialize communications **/
-#define COMM   0x02
-/** @brief Initialize the orb **/
-#define ORB    0x04
-/** @brief Initialize the motors **/
-#define MOTORS 0x08
-/** @brief Initialize the servos **/
-#define SERVOS 0x10
-/** @brief Initialize I2C **/
-#define I2C    0x20
-/** @brief Initialize the buzzer **/
-#define BUZZER 0x40
-/** @brief Initialize the LCD screen **/
-#define LCD    0x80
-/** @brief Initialize everything **/
-#define ALL_ON 0xFF
-
-
-/** @brief Initialize the board **/
-void dragonfly_init(int config);
-
-/** @} **/ //end addtogroup
-
-#include <inttypes.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <avr/io.h>
-#include <avr/interrupt.h>
-#include <util/delay.h>
-#include <util/twi.h>
-
-#include <analog.h>
-#include <dio.h>
-#include <time.h>
-#include <lcd.h>
-#include <lights.h>
-#include <motor.h>
-#include <serial.h>
-#include <buzzer.h>
-#include <rangefinder.h>
-#include <bom.h>
-#include <move.h>
-#include <reset.h>
-#include <math.h>
-
-#include <spi.h>
-#include <encoders.h>
-
-#endif
-
+/**
+ * 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 dragonfly_lib.h
+ * @brief Contains other include files
+ * 
+ * Include this file for all the functionality of libdragonfly.
+ *
+ * @author Colony Project, CMU Robotics Club
+ **/
+
+#ifndef _DRAGONFLY_LIB_H_
+#define _DRAGONFLY_LIB_H_
+
+/**
+ * @addtogroup dragonfly
+ * @{
+ **/
+
+// Configuration definitions
+/** @brief Initialize analog **/
+#define ANALOG 0x01
+/** @brief Initialize serial communications **/
+#define SERIAL 0x02
+/** @brief Initialize USB communications **/
+#define USB    0x02
+/** @brief Initialize communications **/
+#define COMM   0x02
+/** @brief Initialize the orb **/
+#define ORB    0x04
+/** @brief Initialize the motors **/
+#define MOTORS 0x08
+/** @brief Initialize I2C **/
+#define I2C    0x20
+/** @brief Initialize the buzzer **/
+#define BUZZER 0x40
+/** @brief Initialize the LCD screen **/
+#define LCD    0x80
+/** @brief Initialize the rangefinders **/
+#define RANGE  0x0100
+/** @brief Initialize the BOM **/
+#define BOM  0x0200
+/** @brief Initialize everything  **/
+#define ALL_ON 0x03FF
+
+/** @brief Initialize the board **/
+void dragonfly_init(int config);
+
+/** @} **/ //end addtogroup
+
+#include <inttypes.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <util/delay.h>
+#include <util/twi.h>
+
+// This file is included from the libdragonfly directory because it seems to be
+// missing from the AVR libc distribution.
+#include "atomic.h"
+
+#include <analog.h>
+#include <dio.h>
+#include <time.h>
+#include <lcd.h>
+#include <lights.h>
+#include <motor.h>
+#include <serial.h>
+#include <buzzer.h>
+#include <rangefinder.h>
+#include <bom.h>
+#include <move.h>
+#include <reset.h>
+#include <math.h>
+#include <eeprom.h>
+#include <stdbool.h>
+
+/** @brief shortcut for ATOMIC_BLOCK(ATOMIC_RESTORESTATE) **/
+#define SYNC ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
+
+/** @brief atomically grab a lock if it is free, return otherwise **/
+#define REQUIRE_LOCK_OR_RETURN(LOCK) do { SYNC { if (LOCK) return; LOCK=1; } } while (0)
+
+/** @brief atomically release a lock **/
+#define RELEASE_LOCK(LOCK) do { LOCK=0; } while (0)
+
+
+
+#endif
+
Index: branches/encoders/code/lib/include/libdragonfly/atomic.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/atomic.h	(nonexistent)
+++ branches/encoders/code/lib/include/libdragonfly/atomic.h	(revision 1345)
@@ -0,0 +1,309 @@
+/* This file is in the dragonfly library directory because it seems to be
+   missing from the AVR libc distribution */
+   
+/* Copyright (c) 2007 Dean Camera
+   All rights reserved.
+
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are met:
+
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in
+     the documentation and/or other materials provided with the
+     distribution.
+
+   * Neither the name of the copyright holders nor the names of
+     contributors may be used to endorse or promote products derived
+     from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+  POSSIBILITY OF SUCH DAMAGE.
+*/
+
+/* $Id: atomic.h,v 1.3 2007/12/20 14:17:56 joerg_wunsch Exp $ */
+
+#ifndef _UTIL_ATOMIC_H_
+#define _UTIL_ATOMIC_H_ 1
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+
+#if !defined(__DOXYGEN__)
+/* Internal helper functions. */
+static __inline__ uint8_t __iSeiRetVal(void)
+{
+    sei();
+    return 1;
+}
+
+static __inline__ uint8_t __iCliRetVal(void)
+{
+    cli();
+    return 1;
+}
+
+static __inline__ void __iSeiParam(const uint8_t *__s)
+{
+    sei();
+    __asm__ volatile ("" ::: "memory");
+    (void)__s;
+}
+
+static __inline__ void __iCliParam(const uint8_t *__s)
+{
+    cli();
+    __asm__ volatile ("" ::: "memory");
+    (void)__s;
+}
+
+static __inline__ void __iRestore(const  uint8_t *__s)
+{
+    SREG = *__s;
+    __asm__ volatile ("" ::: "memory");
+}
+#endif	/* !__DOXYGEN__ */
+
+/** \file */
+/** \defgroup util_atomic <util/atomic.h> Atomically and Non-Atomically Executed Code Blocks
+
+    \code
+    #include <util/atomic.h>
+    \endcode
+
+    \note The macros in this header file require the ISO/IEC 9899:1999
+    ("ISO C99") feature of for loop variables that are declared inside
+    the for loop itself.  For that reason, this header file can only
+    be used if the standard level of the compiler (option --std=) is
+    set to either \c c99 or \c gnu99.
+
+    The macros in this header file deal with code blocks that are
+    guaranteed to be excuted Atomically or Non-Atmomically.  The term
+    "Atomic" in this context refers to the unability of the respective
+    code to be interrupted.
+
+    These macros operate via automatic manipulation of the Global
+    Interrupt Status (I) bit of the SREG register. Exit paths from
+    both block types are all managed automatically without the need
+    for special considerations, i. e. the interrupt status will be
+    restored to the same value it has been when entering the
+    respective block.
+
+    A typical example that requires atomic access is a 16 (or more)
+    bit variable that is shared between the main execution path and an
+    ISR.  While declaring such a variable as volatile ensures that the
+    compiler will not optimize accesses to it away, it does not
+    guarantee atomic access to it.  Assuming the following example:
+
+    \code
+#include <inttypes.h>
+#include <avr/interrupt.h>
+#include <avr/io.h>
+
+volatile uint16_t ctr;
+
+ISR(TIMER1_OVF_vect)
+{
+  ctr--;
+}
+
+...
+int
+main(void)
+{
+   ...
+   ctr = 0x200;
+   start_timer();
+   while (ctr != 0)
+     // wait
+       ;
+   ...
+}
+    \endcode
+
+    There is a chance where the main context will exit its wait loop
+    when the variable \c ctr just reached the value 0xFF.  This happens
+    because the compiler cannot natively access a 16-bit variable
+    atomically in an 8-bit CPU.  So the variable is for example at
+    0x100, the compiler then tests the low byte for 0, which succeeds.
+    It then proceeds to test the high byte, but that moment the ISR
+    triggers, and the main context is interrupted.  The ISR will
+    decrement the variable from 0x100 to 0xFF, and the main context
+    proceeds.  It now tests the high byte of the variable which is
+    (now) also 0, so it concludes the variable has reached 0, and
+    terminates the loop.
+
+    Using the macros from this header file, the above code can be
+    rewritten like:
+
+    \code
+#include <inttypes.h>
+#include <avr/interrupt.h>
+#include <avr/io.h>
+#include <util/atomic.h>
+
+volatile uint16_t ctr;
+
+ISR(TIMER1_OVF_vect)
+{
+  ctr--;
+}
+
+...
+int
+main(void)
+{
+   ...
+   ctr = 0x200;
+   start_timer();
+   sei();
+   uint16_t ctr_copy;
+   do
+   {
+     ATOMIC_BLOCK(ATOMIC_FORCEON)
+     {
+       ctr_copy = ctr;
+     }
+   }
+   while (ctr_copy != 0);
+   ...
+}
+    \endcode
+
+    This will install the appropriate interrupt protection before
+    accessing variable \c ctr, so it is guaranteed to be consistently
+    tested.  If the global interrupt state were uncertain before
+    entering the ATOMIC_BLOCK, it should be executed with the
+    parameter ATOMIC_RESTORESTATE rather than ATOMIC_FORCEON.
+
+*/
+
+/** \def ATOMIC_BLOCK(type)
+    \ingroup util_atomic
+
+    Creates a block of code that is guaranteed to be executed
+    atomically. Upon entering the block the Global Interrupt Status
+    flag in SREG is disabled, and re-enabled upon exiting the block
+    from any exit path.
+
+    Two possible macro parameters are permitted, ATOMIC_RESTORESTATE
+    and ATOMIC_FORCEON.
+*/
+#if defined(__DOXYGEN__)
+#define ATOMIC_BLOCK(type)
+#else
+#define ATOMIC_BLOCK(type) for ( type, __ToDo = __iCliRetVal(); \
+	                       __ToDo ; __ToDo = 0 )
+#endif	/* __DOXYGEN__ */
+
+/** \def NONATOMIC_BLOCK(type)
+    \ingroup util_atomic
+
+    Creates a block of code that is executed non-atomically. Upon
+    entering the block the Global Interrupt Status flag in SREG is
+    enabled, and disabled upon exiting the block from any exit
+    path. This is useful when nested inside ATOMIC_BLOCK sections,
+    allowing for non-atomic execution of small blocks of code while
+    maintaining the atomic access of the other sections of the parent
+    ATOMIC_BLOCK.
+
+    Two possible macro parameters are permitted,
+    NONATOMIC_RESTORESTATE and NONATOMIC_FORCEOFF.
+*/
+#if defined(__DOXYGEN__)
+#define NONATOMIC_BLOCK(type)
+#else
+#define NONATOMIC_BLOCK(type) for ( type, __ToDo = __iSeiRetVal(); \
+	                          __ToDo ;  __ToDo = 0 )
+#endif	/* __DOXYGEN__ */
+
+/** \def ATOMIC_RESTORESTATE
+    \ingroup util_atomic
+
+    This is a possible parameter for ATOMIC_BLOCK. When used, it will
+    cause the ATOMIC_BLOCK to restore the previous state of the SREG
+    register, saved before the Global Interrupt Status flag bit was
+    disabled. The net effect of this is to make the ATOMIC_BLOCK's
+    contents guaranteed atomic, without changing the state of the
+    Global Interrupt Status flag when execution of the block
+    completes.
+*/
+#if defined(__DOXYGEN__)
+#define ATOMIC_RESTORESTATE
+#else
+#define ATOMIC_RESTORESTATE uint8_t sreg_save \
+	__attribute__((__cleanup__(__iRestore))) = SREG
+#endif	/* __DOXYGEN__ */
+
+/** \def ATOMIC_FORCEON
+    \ingroup util_atomic
+
+    This is a possible parameter for ATOMIC_BLOCK. When used, it will
+    cause the ATOMIC_BLOCK to force the state of the SREG register on
+    exit, enabling the Global Interrupt Status flag bit. This saves on
+    flash space as the previous value of the SREG register does not
+    need to be saved at the start of the block.
+
+    Care should be taken that ATOMIC_FORCEON is only used when it is
+    known that interrupts are enabled before the block's execution or
+    when the side effects of enabling global interrupts at the block's
+    completion are known and understood.
+*/
+#if defined(__DOXYGEN__)
+#define ATOMIC_FORCEON
+#else
+#define ATOMIC_FORCEON uint8_t sreg_save \
+	__attribute__((__cleanup__(__iSeiParam))) = 0
+#endif	/* __DOXYGEN__ */
+
+/** \def NONATOMIC_RESTORESTATE
+    \ingroup util_atomic
+
+    This is a possible parameter for NONATOMIC_BLOCK. When used, it
+    will cause the NONATOMIC_BLOCK to restore the previous state of
+    the SREG register, saved before the Global Interrupt Status flag
+    bit was enabled. The net effect of this is to make the
+    NONATOMIC_BLOCK's contents guaranteed non-atomic, without changing
+    the state of the Global Interrupt Status flag when execution of
+    the block completes.
+*/
+#if defined(__DOXYGEN__)
+#define NONATOMIC_RESTORESTATE
+#else
+#define NONATOMIC_RESTORESTATE uint8_t sreg_save \
+	__attribute__((__cleanup__(__iRestore))) = SREG
+#endif	/* __DOXYGEN__ */
+
+/** \def NONATOMIC_FORCEOFF
+    \ingroup util_atomic
+
+    This is a possible parameter for NONATOMIC_BLOCK. When used, it
+    will cause the NONATOMIC_BLOCK to force the state of the SREG
+    register on exit, disabling the Global Interrupt Status flag
+    bit. This saves on flash space as the previous value of the SREG
+    register does not need to be saved at the start of the block.
+
+    Care should be taken that NONATOMIC_FORCEOFF is only used when it
+    is known that interrupts are disabled before the block's execution
+    or when the side effects of disabling global interrupts at the
+    block's completion are known and understood.
+*/
+#if defined(__DOXYGEN__)
+#define NONATOMIC_FORCEOFF
+#else
+#define NONATOMIC_FORCEOFF uint8_t sreg_save \
+	__attribute__((__cleanup__(__iCliParam))) = 0
+#endif	/* __DOXYGEN__ */
+
+#endif
Index: branches/encoders/code/lib/include/libdragonfly/bom.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/bom.h	(revision 1344)
+++ branches/encoders/code/lib/include/libdragonfly/bom.h	(revision 1345)
@@ -1,4 +1,30 @@
 /**
+ * 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 bom.h
  * @brief Definitions for using the BOM
  * 
@@ -16,12 +42,42 @@
  * @addtogroup bom
  * @{
  **/
+ 
+/** @brief Include all elements in the 16-bit bitfield **/
+#define BOM_ALL 0xFFFF
 
-/** @brief Returns the location of the maximum BOM reading. **/
-int get_max_bom( void );
-/** @brief Turns the BOM on. **/
+/** @brief Original BOM - No Range, No Individual LED control **/
+#define BOM10     0
+
+/** @brief BOM 1.5 - No Range, Individual LED control **/
+#define BOM15   1
+
+/** @brief RBOM - Range, Individual LED control **/
+#define RBOM    2
+
+
+/** @brief Initialize the bom according to bom type **/
+void bom_init(char type);
+
+/** @brief Refresh bom_val[] with new values from analog8.  analog_init and bom_init must be called for this to work. **/
+void bom_refresh(int bit_field);
+
+/** @brief Gets the bom reading from bom_val[which].  Call bom_refresh beforehand to read new bom values. **/
+int bom_get(int which);
+
+/** @brief Compares all the values in bom_val[] and returns the index to the highest value element. **/
+int bom_get_max(void);
+
+/** @brief Enables the selected bom leds on a BOM1.5 **/
+void bom_set_leds(int bit_field);
+
+/** @brief (DEPRECATED) Gets and compares all bom values.  Returns the index to the highest value element. **/
+int get_max_bom(void);
+
+/** @brief Turns on all BOM leds, or turns on enabled leds on a BOM1.5. **/
 void bom_on(void);
-/** @brief Turns the BOM off. **/
+
+/** @brief Turns off all bom leds. **/
 void bom_off(void);
 
 /** @} **/
Index: branches/encoders/code/lib/include/libdragonfly/i2c.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/i2c.h	(revision 1344)
+++ branches/encoders/code/lib/include/libdragonfly/i2c.h	(revision 1345)
@@ -1,8 +1,36 @@
+/**
+ * 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 i2c.h
  *  @brief Header file for I2C
  *
- *  @author CMU Robotics Club, Kevin Woo, Suresh Nidhiry
- */
+ *  Contains functions for I2C.
+ *
+ *  @author Kevin Woo and Suresh Nidhiry, Colony Project, CMU Robotics Club
+ **/
 
 
 #ifndef _I2C_H_
@@ -10,13 +38,13 @@
 
 #include <stddef.h>
 
-/** @brief Address of slave receive handler function */
+/** @brief Address of slave receive handler function **/
 typedef void (*fun_srecv_t)(char);
 
-/** @brief Address of master receive handler function*/
+/** @brief Address of master receive handler function**/
 typedef int (*fun_mrecv_t)(char);
 
-/** @brief Address of slave send handler function*/
+/** @brief Address of slave send handler function**/
 typedef char (*fun_send_t)(void);
 
 int i2c_init(char addr, fun_mrecv_t master_recv, fun_srecv_t slave_recv, fun_send_t slave_send);
@@ -26,3 +54,4 @@
 void i2c_packet_rec (char i2c_byte);
 void i2c_packet_sniff(char data);
 #endif
+
Index: branches/encoders/code/lib/include/libdragonfly/serial.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/serial.h	(revision 1344)
+++ branches/encoders/code/lib/include/libdragonfly/serial.h	(revision 1345)
@@ -1,4 +1,30 @@
 /**
+ * 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 serial.h
  * @brief Contains declarations for serial input and output
  *
@@ -10,7 +36,7 @@
  **/
 
 /*
-	serial.h - Contains definitions and function prototypes for the RS232 serial port
+  serial.h - Contains definitions and function prototypes for the RS232 serial port
   author(s): pkv
   
   Directions:
@@ -19,13 +45,13 @@
   write characters to the serial ports.
   
   UART Mapping:
-    usb_*() -> UART0
-    xbee_*() -> UART1
+  usb_*() -> UART0
+  xbee_*() -> UART1
   
   Options: (Add the following defines to your code to configure this library)
-    #define USB_BAUD { 115200 | 9600 } <= pick ONE value from in here
-    #define XBEE_BAUD { 115200 | 9600 } <= pick ONE value from in here
-    #define USE_STDIO
+  #define USB_BAUD { 115200 | 9600 } <= pick ONE value from in here
+  #define XBEE_BAUD { 115200 | 9600 } <= pick ONE value from in here
+  #define USE_STDIO
   
   Note: If you enable USE_STDIO, the first init function that is called will 
   automatically be linked to stdin, stdout, and stderr.  To use the baud rate 
@@ -42,6 +68,9 @@
 #ifndef _SERIAL_H
 #define _SERIAL_H
 
+#include <inttypes.h>
+#include <avr/pgmspace.h>
+
 /**
  * @defgroup usb USB Input / Output
  * @brief Functions for USB input / output
@@ -67,9 +96,20 @@
 int usb_getc_nb(char *c);
 /** @brief Print a string to USB **/
 int usb_puts(char *s);
+/** @brief Print a string from program space to USB **/
+void usb_puts_P (PGM_P s);
 /** @brief Print an integer to USB **/
 int usb_puti(int value);
+/** @brief Determine a hexadecimal digit **/
+uint8_t hex_digit (uint8_t value);
+/** @brief Print a fixed width hexadecimal representation to USB **/
+void usb_puth16 (uint16_t value);
+/** @brief Print a fixed width hexadecimal representation to USB **/
+void usb_puth8(uint8_t value);
+/** @brief Alias for usb_puth16 **/
+static inline void usb_puth (uint16_t value) { usb_puth16 (value); };
 
+
 /** @} **/ //end addtogroup
 
 /**
@@ -98,6 +138,8 @@
 /** @brief Read a character from the XBee without blocking **/
 int xbee_getc_nb(char *c);
 
+
+
 /** @} **/ //end addtogroup
 
 #endif
Index: branches/encoders/code/lib/include/libdragonfly/buzzer.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/buzzer.h	(revision 1344)
+++ branches/encoders/code/lib/include/libdragonfly/buzzer.h	(revision 1345)
@@ -1,4 +1,30 @@
 /**
+ * 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 buzzer.h
  * @brief Contains definitions for using the buzzer.
  *
Index: branches/encoders/code/lib/include/libdragonfly/reset.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/reset.h	(revision 1344)
+++ branches/encoders/code/lib/include/libdragonfly/reset.h	(revision 1345)
@@ -1,10 +1,36 @@
 /**
+ * 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 reset.h
  * @brief Contains function(s) for resetting the robots
  * 
  * @author Colony Project, CMU Robotics Club, James Kong
-*/
+ */
 
 #ifndef _RESET_H_
 #define _RESET_H_
Index: branches/encoders/code/lib/include/libdragonfly/rangefinder.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/rangefinder.h	(revision 1344)
+++ branches/encoders/code/lib/include/libdragonfly/rangefinder.h	(revision 1345)
@@ -1,4 +1,30 @@
 /**
+ * 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 rangefinder.h
  * @brief Contains rangefinder declarations and functions
  * 
Index: branches/encoders/code/lib/include/libdragonfly/eeprom.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/eeprom.h	(nonexistent)
+++ branches/encoders/code/lib/include/libdragonfly/eeprom.h	(revision 1345)
@@ -0,0 +1,49 @@
+/**
+ * @file eeprom.h
+ * @brief handles eeprom storage for persistent data
+ *
+ * Contains functions and definitions for reading and writing to eeprom
+ *
+ * @author Colony Project, Brad Neuman
+ */
+ 
+ #ifndef _EEPROM_H_
+ #define _EEPROM_H_
+ 
+ #include <bom.h>
+ 
+ #define EEPROM_ROBOT_ID_ADDR 0x10
+ #define EEPROM_BOM_TYPE_ADDR 0x14
+ 
+ /** @brief store a byte to eeproem
+  *  @return 0 if success, nonzero on failure
+  */
+ int eeprom_put_byte(unsigned int addr, unsigned char byte);
+ 
+ /** @brief reads a byte from eeprom
+  *
+  *  Pass it thge address and a pointer to a byte where the byte at the
+  *  address will be stored
+  *
+  *  @return 0 if successful (byte is set to the eeprom value at addr), 
+  *  nonzero if there was a problem
+   */
+ int eeprom_get_byte(unsigned int addr, unsigned char *byte);
+ 
+ /** @brief get stored robot ID
+  *
+  *  checks that EEPROM has been programed with an ID and returns it
+  *
+  *  @return the robot id, if it is stored. If it returns 0xFF it is probably invalid
+  */
+ unsigned char get_robotid(void);
+  
+   /** @brief get stored robot ID
+  *
+  * checks that EEPROM has been programed with an BOM type and returns it
+  *
+  *  @return the robot bom type as defined in bom.h, if it is stored. If it returns 0xFF it is probably invalid
+  */
+ unsigned char get_bom_type(void);
+ 
+ #endif
Index: branches/encoders/code/lib/include/libdragonfly/ring_buffer.h
===================================================================
--- branches/encoders/code/lib/include/libdragonfly/ring_buffer.h	(revision 1344)
+++ branches/encoders/code/lib/include/libdragonfly/ring_buffer.h	(revision 1345)
@@ -1,12 +1,39 @@
-/** @file Include macros to create a ring buffer. Originally written for 15-410.
- *  Modified for use in ATMega128 serial driver
+/**
+ * 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 ring_buffer.h
+ * @brief Ring Buffer
  *
- *	@author Cornell Wright (cgwright)
- *	@author Jason P. Winters (jpwinter)
+ * Include macros to create a ring buffer. Originally written for 15-410.
+ * Modified for use in ATMega128 serial driver
  *
- *	@bugs Could not create buffer of non byte-sized elements due to modding with sizeof(queue).  
- * 		  Fixed 3/2/07, Felix Duvallet and Brad Neuman
- */
+ * @author Cornell Wright (cgwright)
+ * @author Jason P. Winters (jpwinter)
+ *
+ **/
 
 #ifndef _RING_BUFFER_H
 #define _RING_BUFFER_H
Index: branches/encoders/code/lib/src/libdragonfly/encoders.c
===================================================================
--- branches/encoders/code/lib/src/libdragonfly/encoders.c	(revision 1344)
+++ branches/encoders/code/lib/src/libdragonfly/encoders.c	(revision 1345)
@@ -19,10 +19,6 @@
 int right_dx;
 long int timecount;
 
-int left_v;
-int right_v;
-
-
 volatile short int data_ready;
 
 void encoder_recv(char data);
@@ -38,38 +34,23 @@
 inline unsigned int right_data_array_prev(void);
 inline unsigned int right_data_array_bottom(void);
 
-//RING_BUFFER_NEW(enc_buffer, BUFFER_SIZE, short int);
 
-void encoder_recv_complete(void);
-
 void encoder_recv_complete(){
-   encoder_buf_index = 0;
-   //  usb_puts("[");usb_puti(left_dx);usb_puts(",");usb_puti(right_dx);usb_puts("]\r\n");
-   //   usb_puts("\r\n");
-  data_ready++;
+  	encoder_buf_index = 0;
+	data_ready++;
   
-  //delay_ms(ENCODER_DELAY);
-
-   spi_transfer(5);
+   	spi_transfer(5);
 }
 
-void put_bin(char data){
-  int i;
-
-  for(i=7;i>=0;i--)
-    usb_puti((data>>i)&1);
-  usb_puts(" ");
-}
-
-/**
- * Initializes the encoders variables
- **/
+/** 
+* @brief Initializes encoder variables and the hardware interface.
+*/
 void encoders_init(void){
 	int i;
 	
 	data_ready=0;
 
-	spi_init(encoder_recv/*put_bin*/, encoder_recv_complete);
+	spi_init(encoder_recv, encoder_recv_complete);
 	encoder_buf_index = 0;
 	left_data_buf = 0;
 	right_data_buf= 0;
@@ -76,10 +57,6 @@
 	left_data = -1;
 	right_data = -1;
 	
-	
-	left_v=0;
-	right_v=0;
-	
 	//RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE);
 	left_data_idx = 0;
 	right_data_idx = 0;
@@ -90,11 +67,10 @@
 		right_data_array[i] = 0;
 	}
 	spi_transfer(5);
-	usb_puts("\tencoders.c Debug: ENCODERS INITIALIZED\n\r");
 }
 
 /**
- * Returns the specified encoders value
+ * @brief Returns the specified encoders value
  *
  * @param encoder this is the encoder that you want to read. Valid arguments
  *          are LEFT and RIGHT
@@ -102,100 +78,138 @@
  * @return the value of the specified encoder
  **/
 int encoder_read(char encoder){
-	if(encoder==LEFT){
-		usb_puts("\tencoders.c Debug: PRINTING LEFT ENCODER VALUE\n\r");
-		return left_data;
-	}
-	else if(encoder==RIGHT){
-		usb_puts("\tencoders.c Debug: PRINTING RIGHT ENCODER VALUE\n\r");
-		return right_data;
-	}
-	else{
-		usb_puts("\tencoders.c Debug: INVALID ENCODER - USE LEFT/RIGHT\n\r");
-		 return -1;
-	}
+	
+	if(encoder==LEFT) return left_data;
+	else if(encoder==RIGHT) return right_data;
+	else return -1;
 }
 
-int encoder_change(char encoder){
-	return 0;
-}
 
+/** 
+* @brief Outputs encoder direction as FORWARD OR BACK
+* A STUB! DO NOT use.
+* 
+* @param encoder The encoder you want the direction of.
+* Valid arguments are right and left.
+* 
+* @return FORWARD or BACK (the constants)
+*/
 char encoder_direction(char encoder){
 	return 0;
 }
+
 /**
- * Gets the velocity of the specified encoder.
+ * Gets the total distance covered by the specified encoder (in encoder clicks)
  *
  * @param encoder the encoder that you want to read, use LEFT or RIGHT
  *
- * @return The velocity of the specified encoder.
+ * @return The distance covered by the specified encoder.
  **/
 int encoder_get_dx(char encoder) {
-	if(encoder==LEFT)
-		return left_dx;
-	else if(encoder==RIGHT)
-		return right_dx;
+	
+	if(encoder==LEFT) return left_dx;
+	else if(encoder==RIGHT) return right_dx;
 	else return -1;
 }
 
 /**
- * Resets the value of the velocity global variable for the specified
+ * Resets the distance accumulator for the specified
  *  encoder.
  *
- * @param encoder the encoder that you want to modify
+ * @param encoder the encoder that you want to reset distance for
  **/
 void encoder_rst_dx(char encoder) {
-	if(encoder==LEFT)
-		left_dx = 0;
-	else if(encoder==RIGHT)
-		right_dx = 0;
+	
+	if(encoder==LEFT) left_dx = 0;
+	else if(encoder==RIGHT) right_dx = 0;
 }
 
-/**
- * Returns the current time count for the encoders.
- **/
+/** 
+* @brief Returns the number of encoder reads that have occurred.
+* 
+* @return The time count.
+*/
 int encoder_get_tc(void) {
 	return timecount;
 }
 
-/**
- * Resets the time count for the encoders.
- **/
+/** 
+* @brief Resets the encoder read counter.
+*/
 void encoder_rst_tc(void) {
 	timecount = 0;
 }
 
+///////////////////////////////////////////////////////////////////////
+int left_data_at(int index) {
+	int tmp_idx = left_data_idx - index;
+	if (tmp_idx < 0)
+		tmp_idx += BUFFER_SIZE;
+	return left_data_array[tmp_idx];
+}
 
+int right_data_at(int index) {
+	int tmp_idx = right_data_idx - index;
+	if (tmp_idx < 0)
+		tmp_idx += BUFFER_SIZE;
+	return right_data_array[tmp_idx];	
+}
+
+
+int get_dx(char encoder, int index) {
+	int dx, ctr;
+	ctr = 0;
+	dx = 1024;
+	do {
+		if (encoder == LEFT)
+			dx = left_data_at(index+ctr+38) - left_data_at(index+ctr);
+		else
+			dx = right_data_at(index+ctr) - right_data_at(index+ctr+38);
+		ctr++;
+	} while ((dx > 30 || dx < -30) && ctr < 3);
+	if (dx > 30 || dx < -30)
+		return ERR_VEL;
+	return dx;
+}
+/** 
+* @brief Returns the approximated instantaneous velocity of the robot
+* in terms of encoder clicks.
+* 
+* @param encoder RIGHT or LEFT - the wheel you want the velocity for.
+* 
+* @return The instantaneous velocity for the given wheel.
+*/
 int encoder_get_v(char encoder){
-	int last, res=0;
-	
-	cli();
-	
-	if(encoder==LEFT){
-		if(left_data_idx==0)
-			last = BUFFER_SIZE - 1;
-		else
-			last = left_data_idx - 1;
-		res = ((int)left_data_array[last]) - ((int)left_data_array[left_data_idx]);
-	}
-	if(encoder==RIGHT){
-		if(right_data_idx==0)
-			last = BUFFER_SIZE - 1;
-		else
-			last = right_data_idx - 1;
-		res = ((int)right_data_array[right_data_idx]) - ((int)right_data_array[last]);
-	}
-	
-	sei();
+	/*
+	if (encoder == LEFT)
+		return left_data_array_bottom() - left_data_array_top();
 
-	while(res<MIN_V)//underflow
-		res+=1024;
-	while(res>MAX_V)//overflow
-		res-=1024;
+	if (encoder == RIGHT)
+		return right_data_array_top() - right_data_array_bottom();
 
-	return res;
+	return -1;
+	*/
+	int vel1, vel2, tmp;
+	vel1 = get_dx(encoder, 0);
+	vel2 = get_dx(encoder, 1);
+
+	if (vel1 == ERR_VEL && vel2 == ERR_VEL)
+		return ERR_VEL << 1;
+	else if (vel2 == ERR_VEL)
+		return vel1 << 1;
+	else if (vel1 == ERR_VEL) 
+		return vel2 << 1;
+	else
+		return vel1 + vel2;
 }
+/////////////////////////////////////////////////////////////////////////
 
+/** 
+* @brief Waits until n encoder reads have occurred.
+* Counter is reset on functions exit.
+* 
+* @param n 
+*/
 void encoder_wait(int n){
 	while(data_ready<n);
 	data_ready=0;
@@ -228,39 +242,44 @@
 	encoder_buf_index = (encoder_buf_index + 1) % 5;
 
 	if(encoder_buf_index==0) {
-		  
-		if(left_data_buf & (COF | LIN) || !(left_data_buf & OCF))  left_data = INVALID;
-		else if(((left_data_buf & MagINCn) > 0)  && ((left_data_buf & MagDECn) > 0)) left_data = MAGNET_FAILURE;
+		
+		/*Error handling for the left encoder*/ 
+		if(!(left_data_buf & OCF)) 
+			left_data = ENCODER_DATA_NOT_READY; 
+		if(left_data_buf & (COF | LIN))  
+			left_data = ENCODER_MISALIGNED;
+		else if((left_data_buf & MagINCn) && (left_data_buf & MagDECn)) 
+			left_data = ENCODER_MAGNET_FAILURE;
 		else left_data = (left_data_buf>>5) & 1023;
 			
-		if(right_data_buf & (COF | LIN) || !(right_data_buf & OCF)) right_data = INVALID;
-		else if ( ((right_data_buf & MagINCn) > 0)  && ((right_data_buf & MagDECn) > 0)) right_data = MAGNET_FAILURE;
+		/*Error handling for the right encoder*/ 
+	 	if(!(right_data_buf & OCF))
+			right_data = ENCODER_DATA_NOT_READY;	
+		if(right_data_buf & (COF | LIN)) 
+			right_data = ENCODER_MISALIGNED;
+		else if ((right_data_buf & MagINCn)  && (right_data_buf & MagDECn)) 
+			right_data = ENCODER_MAGNET_FAILURE;
 		else right_data = (right_data_buf>>5) & 1023;
 			
   		left_data_buf = 0;
 		right_data_buf = 0;
 
-		
-		if(left_data != INVALID && left_data != MAGNET_FAILURE) {
+		/*Above 1023 is invalid data*/	
+		if(!(left_data > 1023)) {
 			left_data_array_put(left_data);
 
 			//Adjust left accumulator
-			dx = left_data_array_prev() - left_data;
+			dx = - left_data + left_data_array_prev();
 			
-			//Adjust velocity: save last dx
-			left_v = left_dx;
-
 			if(left_data_array_prev()==0)  dx=0;
 
 			if(dx > 512) left_dx += dx - 1023; //Underflow
 			else if(dx < -512) left_dx += dx + 1023; //Overflow
 			else left_dx += dx;
-			
-			//Adjust velocity: update
-			left_v = left_dx - left_v;
 		}
 
-		if(right_data != INVALID && right_data != MAGNET_FAILURE) {
+		/*Above 1023 is invalid data*/	
+		if(!(right_data > 1023)) {
 			right_data_array_put(right_data);
 
 			//Adjust right accumulator
@@ -279,27 +298,6 @@
 }
 
 
-void encoders_print_data_array(void){
-	int i;
-	usb_puts("Left Data: Pointer is ");
-	usb_puti(left_data_idx);
-	usb_puts("\n\r");
-	for(i=0; i<BUFFER_SIZE; i++){
-		usb_puti(left_data_array[i]);
-		usb_putc(' ');
-	}
-	usb_puts("\r\n");
-	usb_puts("Right Data: Pointer is");
-	usb_puti(right_data_idx);
-	usb_puts("\n\r");
-	for(i=0; i<BUFFER_SIZE; i++){
-		usb_puti(right_data_array[i]);
-		usb_putc(' ');
-	}
-	usb_puts("\r\n\r\n");
-}
-
-
 //Helper Functions
 inline void left_data_array_put(unsigned short int value) {
 	if(left_data_idx == BUFFER_SIZE-1)
Index: branches/encoders/code/lib/src/libdragonfly/spi.c
===================================================================
--- branches/encoders/code/lib/src/libdragonfly/spi.c	(revision 1344)
+++ branches/encoders/code/lib/src/libdragonfly/spi.c	(revision 1345)
@@ -32,7 +32,7 @@
     spi_recv_func = recv_func;
     spi_recv_complete_func = recv_complete_func;
     spi_bytes = 0;
-    usb_puts("\tspi.c Debug: SPI INITIALIZED\n");
+    //usb_puts("\tspi.c Debug: SPI INITIALIZED\n");
 }
 
 /* Transfer a given byte to slave and receive a byte */
Index: branches/encoders/code/lib/bin/libwireless.a
===================================================================
Cannot display: file marked as a binary type.
svn:mime-type = application/octet-stream
Index: branches/encoders/code/lib/bin/libdragonfly.a
===================================================================
Cannot display: file marked as a binary type.
svn:mime-type = application/octet-stream
Index: branches/encoders/code/projects/libwireless/lib/queue.c
===================================================================
--- branches/encoders/code/projects/libwireless/lib/queue.c	(revision 1344)
+++ branches/encoders/code/projects/libwireless/lib/queue.c	(nonexistent)
@@ -1,152 +0,0 @@
-#include "queue.h"
-#include <wl_defs.h>
-
-#include <stdlib.h>
-#include <stdio.h>
-
-/**
- * @struct node_def
- * A node in the queue.
- **/
-typedef struct node_def
-{
-	/** The item at this point in the queue. **/
-	void* val;
-	/** The next node in the queue. **/
-	struct node_def* next;
-} Node;
-
-/**
- * Create a queue.
- *
- * @return the newly created queue.
- **/
-Queue* queue_create()
-{
-	Queue* q = (Queue*)malloc(sizeof(Queue));
-	q->head = NULL;
-	q->size = 0;
-	return q;
-}
-
-/**
- * Destroys a queue, freeing memory.
- *
- * @param q the queue to destroy
- **/
-void queue_destroy(Queue* q)
-{
-	Node* t = q->head;
-	while (t != NULL)
-	{
-		Node* t2 = t->next;
-		free(t);
-		t = t2;
-	}
-	free(q);
-}
-
-/**
- * Add an element to a queue.
- *
- * @param q the queue to add an element to
- * @param item the item to add to the queue
- **/
-void queue_add(Queue* q, void* item)
-{
-	Node* n = (Node*)malloc(sizeof(Node));
-	n->val = item;
-	n->next = NULL;
-	
-	if (q->head == NULL)
-	{
-		q->head = n;
-		q->tail = n;
-	}
-	else
-	{
-		q->tail->next = n;
-		q->tail = n;
-	}
-	
-	q->size++;
-}
-
-/**
- * Remove an element from the front of a queue.
- *
- * @param q the queue to remove the element from
- *
- * @return the element which was removed
- **/
-void* queue_remove(Queue* q)
-{
-	Node* first = q->head;
-	if (first == NULL)
-	{
-		WL_DEBUG_PRINT("Attempting to remove item \
-			from empty queue.\r\n");
-		WL_DEBUG_PRINT_INT(queue_size(q));
-	}
-	void* ret = first->val;
-	q->head = first->next;
-	if (q->tail == first)
-		q->tail = NULL;
-	free(first);
-	q->size--;
-	return ret;
-}
-
-/**
- * Remove all instances of a given element from a queue.
- *
- * @param q the queue to remove the elements from
- * @param item the element to remove all instances of
- **/
-void queue_remove_all(Queue* q, void* item)
-{
-	Node* curr = q->head;
-	Node* prev = NULL;
-	
-	while (curr != NULL)
-	{
-		Node* next = curr->next;
-		if (curr->val == item)
-		{
-			if (q->head == curr)
-				q->head = next;
-			if (q->tail == curr)
-				q->tail = prev;
-			if (prev != NULL)
-				prev->next = next;
-			free(curr);
-			q->size--;
-		}
-		else
-			prev = curr;
-		curr = next;
-	}
-}
-
-/**
- * Get the number of elements in the queue.
- *
- * @param q the queue to get the size of
- * @return the size of the queue
- **/
-int queue_size(Queue* q)
-{
-	return q->size;
-}
-
-/**
- * Check if the queue is empty.
- * 
- * @param q the queue to check
- * @return nonzero if the queue is empty, 0 otherwise
- **/
-int queue_is_empty(Queue* q)
-{
-	return q->size == 0;
-}
-
Index: branches/encoders/code/projects/libwireless/lib/queue.h
===================================================================
--- branches/encoders/code/projects/libwireless/lib/queue.h	(revision 1344)
+++ branches/encoders/code/projects/libwireless/lib/queue.h	(nonexistent)
@@ -1,57 +0,0 @@
-/**
- * @file queue.h
- * @brief A queue implementation
- * 
- * Implements a queue, a first in, first out data structure.
- *
- * @author Brian Coltin, Colony Project
- **/
-
-struct node_def;
-
-/**
- * @defgroup queue Queue
- * @brief A queue implementation
- * 
- * A queue implementation.
- *
- * @{
- **/
-
-/**
- * @struct Queue
- * Represents a queue, a first in, first out data structure.
- **/
-typedef struct
-{
-	/**
-	 * The head of the queue, the next item to be removed.
-	 **/
-	struct node_def* head;
-	/**
-	 * The tail of the queue, the last item added.
-	 **/
-	struct node_def* tail;
-	/**
-	 * The number of elements in the queue.
-	 **/
-	int size;
-} Queue;
-
-/** @brief Create a new queue **/
-Queue* queue_create(void);
-/** @brief Destroy a queue **/
-void queue_destroy(Queue* q);
-/** @brief Add an element to a queue **/
-void queue_add(Queue* q, void* item);
-/** @brief Remove an element from a queue **/
-void* queue_remove(Queue* q);
-/** @brief Remove all instances of a given element from a queue **/
-void queue_remove_all(Queue* q, void* item);
-/** @brief Get the size of a queue **/
-int queue_size(Queue* q);
-/** @brief Check if the queue is empty **/
-int queue_is_empty(Queue* q);
-
-/** @} **/
-
Index: branches/encoders/code/projects/libwireless/lib/wl_token_ring.c
===================================================================
--- branches/encoders/code/projects/libwireless/lib/wl_token_ring.c	(revision 1344)
+++ branches/encoders/code/projects/libwireless/lib/wl_token_ring.c	(revision 1345)
@@ -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
@@ -15,6 +48,8 @@
 #include <time.h>
 #endif
 
+//TODO: why is this in both this file and sensor_matrix.c?  If it is needed in both places,
+// put it in sensor_matrix.h.  This file already includes sensor_matrix.h
 #define DEFAULT_SENSOR_MATRIX_SIZE 20
 
 /*Ring States*/
@@ -26,84 +61,113 @@
 #define LEAVING 4
 
 /*Frame Types*/
-#define TOKEN_JOIN_ACCEPT_FRAME 1
+#define TOKEN_JOIN_ACCEPT_FRAME	1
+#define WL_TOKEN_PASS_FRAME	2
 
 /*Function Prototypes*/
 
 /*Wireless Library Prototypes*/
-void wl_token_ring_timeout_handler(void);
-void wl_token_ring_response_handler(int frame, int received);
-void wl_token_ring_receive_handler(char type, int source, unsigned char* packet,
-							int length);
-void wl_token_ring_cleanup(void);
+static void wl_token_ring_timeout_handler(void);
+static void wl_token_ring_response_handler(int frame, int received);
+static void wl_token_ring_receive_handler(char type, int source, unsigned char* packet, int length);
+static void wl_token_ring_cleanup(void);
 
 /*Helper Functions*/
-void wl_token_pass_token(void);
-int get_token_distance(int robot1, int robot2);
-void wl_token_get_token(void);
+static int wl_token_pass_token(void);
+static int get_token_distance(int robot1, int robot2);
+static void wl_token_get_token(void);
 
 /*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);
+static void wl_token_pass_receive(int source);
+static void wl_token_sensor_matrix_receive(int source, unsigned char* sensorData, int sensorDataLength);
+static void wl_token_bom_on_receive(int source);
+static void wl_token_join_receive(int source);
+static void wl_token_join_accept_receive(int source);
 
 /*Global Variables*/
 
-//the sensor matrix
-SensorMatrix* sensorMatrix;
-
 //the robot we are waiting to say it has received the token. -1 if unspecified
-int wl_token_next_robot = -1;
+static int wl_token_next_robot = -1;
 
 //true if the robot should be in the token ring, 0 otherwise
-int ringState = NONMEMBER;
+static int ringState = NONMEMBER;
 //the id of the robot who accepted us into the token ring, only used in ACCEPTED state
-int acceptor = -1;
+static int acceptor = -1;
 //id of the robot we are accepting
-int accepted = -1;
+static int accepted = -1;
 
 //the counter for when we assume a robot is dead
-int deathDelay = -1;
+static 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;
+static int joinDelay = -1;
 
 //current robot to check in the iterator
-int iteratorCount = 0;
+static int iteratorCount = 0;
 
-void do_nothing(void) {}
-int get_nothing(void) {return -1;}
+// the amount of time a robot has had its BOM on for
+static int bom_on_count = 0;
 
+#ifndef ROBOT
+static void do_nothing(void) {}
+static int get_nothing(void) {return -1;}
+#endif
+
 #ifdef ROBOT
 #ifndef FIREFLY
-void (*bom_on_function) (void) = bom_on;
-void (*bom_off_function) (void) = bom_off;
-int (*get_max_bom_function) (void) = get_max_bom;
+static void (*bom_on_function) (void) = bom_on;
+static void (*bom_off_function) (void) = bom_off;
+static int (*get_max_bom_function) (void) = get_max_bom;
 #else
-void (*bom_on_function) (void) = do_nothing;
-void (*bom_off_function) (void) = do_nothing;
-int (*get_max_bom_function) (void) = get_nothing;
+static void (*bom_on_function) (void) = do_nothing;
+static void (*bom_off_function) (void) = do_nothing;
+static int (*get_max_bom_function) (void) = get_nothing;
 #endif
 #else
-void (*bom_on_function) (void) = do_nothing;
-void (*bom_off_function) (void) = do_nothing;
-int (*get_max_bom_function) (void) = get_nothing;
+static void (*bom_on_function) (void) = do_nothing;
+static void (*bom_off_function) (void) = do_nothing;
+static int (*get_max_bom_function) (void) = get_nothing;
 #endif
 
-PacketGroupHandler wl_token_ring_handler =
-		{WL_TOKEN_RING_GROUP, wl_token_ring_timeout_handler,
+static PacketGroupHandler wl_token_ring_handler =
+	{WL_TOKEN_RING_GROUP, wl_token_ring_timeout_handler,
 		wl_token_ring_response_handler, wl_token_ring_receive_handler,
 		wl_token_ring_cleanup};
 
 /**
+ * Causes the robot to join an existing token ring, or create one
+ * if no token ring exists. The token ring uses global and robot to robot
+ * packets, and does not rely on any PAN.
+ **/
+int wl_token_ring_join()
+{
+	WL_DEBUG_PRINT("Joining the token ring.\r\n");
+
+	ringState = JOINING;
+	joinDelay = DEATH_DELAY * 2;
+	if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN, NULL, 0, 0) != 0) {
+		return -1;
+	}
+
+	return 0;
+}
+
+/**
+ * Causes the robot to leave the token ring. The robot stops
+ * alerting others of its location, but continues storing the
+ * locations of other robots.
+ **/
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
+void wl_token_ring_leave()
+{
+	ringState = LEAVING;
+}
+
+/**
  * Initialize the token ring packet group and register it with the
  * wireless library. The robot will not join a token ring.
  **/
-void wl_token_ring_register()
+int wl_token_ring_register()
 {
 	if (wl_get_xbee_id() > 0xFF)
 	{
@@ -112,20 +176,23 @@
 		WL_DEBUG_PRINT("XBee ID must be single byte for token ring, is ");
 		WL_DEBUG_PRINT_INT(wl_get_xbee_id());
 		WL_DEBUG_PRINT(".\r\n");
-		return;
+		return -1;
 	}
-	
-	sensorMatrix = sensor_matrix_create();
-	interrupting = queue_create();
+
+	sensor_matrix_create();
 	//add ourselves to the sensor matrix
-	sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);
+	sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
 
 	wl_register_packet_group(&wl_token_ring_handler);
+
+	return 0;
 }
 
 /**
  * Removes the packet group from the wireless library.
  **/
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
 void wl_token_ring_unregister()
 {
 	wl_unregister_packet_group(&wl_token_ring_handler);
@@ -133,7 +200,7 @@
 
 /**
  * Sets the functions that are called when the BOM ought to be
- * turned on or off. This could be used for things such as 
+ * turned on or off. This could be used for things such as
  * charging stations, which have multiple BOMs.
  *
  * @param on_function the function to be called when the BOM
@@ -154,16 +221,16 @@
 /**
  * Called to cleanup the token ring packet group.
  **/
-void wl_token_ring_cleanup()
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
+static void wl_token_ring_cleanup()
 {
-	sensor_matrix_destroy(sensorMatrix);
-	queue_destroy(interrupting);
 }
 
 /**
  * Called approximately every quarter second by the wireless library.
  **/
-void wl_token_ring_timeout_handler()
+static void wl_token_ring_timeout_handler()
 {
 	//someone is not responding, assume they are dead
 	if (deathDelay == 0)
@@ -172,24 +239,27 @@
 		//also, declare that person dead, as long as it isn't us
 		if (wl_token_next_robot != wl_get_xbee_id())
 		{
-			sensor_matrix_set_in_ring(sensorMatrix, wl_token_next_robot, 0);
+			sensor_matrix_set_in_ring(wl_token_next_robot, 0);
 			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
-		if (ringState == MEMBER)
+		if (ringState == MEMBER) {
 			wl_token_pass_token();
+		}
 	}
 
 	//we must start our own token ring, no one is responding to us
 	if (joinDelay == 0)
 	{
-		if (sensor_matrix_get_joined(sensorMatrix) == 0)
+		if (sensor_matrix_get_joined() == 0)
 		{
 			WL_DEBUG_PRINT("Creating our own token ring, no robots seem to exist.\r\n");
-			sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);
+			sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
 			ringState = MEMBER;
 			//this will make us pass the token to ourself
 			//repeatedly, and other robots when they join
@@ -205,19 +275,26 @@
 		}
 	}
 
-	if (deathDelay >= 0)
+	if (deathDelay >= 0) {
 		deathDelay--;
-	if (joinDelay >= 0)
+	}
+
+	if (joinDelay >= 0) {
 		joinDelay--;
+	}
+
+	if (bom_on_count >= 0) {
+		bom_on_count++;
+	}
 }
 
 /**
  * Called when the XBee tells us if a packet we sent has been received.
- * 
+ *
  * @param frame the frame number assigned when the packet was sent
  * @param received 1 if the packet was received, 0 otherwise
  **/
-void wl_token_ring_response_handler(int frame, int received)
+static void wl_token_ring_response_handler(int frame, int received)
 {
 	if (!received)
 	{
@@ -232,29 +309,20 @@
  * @param packet the data in the packet
  * @param length the length of the packet in bytes
  **/
-void wl_token_ring_receive_handler(char type, int source, unsigned char* packet,
-							int length)
+static void wl_token_ring_receive_handler(char type, int source, unsigned char* packet, int length)
 {
 	switch (type)
 	{
 		case WL_TOKEN_PASS:
-			if (length < 1)
-			{
-				WL_DEBUG_PRINT("Malformed Token Pass packet received.\r\n");
-				return;
-			}
-			wl_token_pass_receive(source, packet[0], packet + 1, length - 1);
+			wl_token_pass_receive(source);
 			break;
+		case WL_TOKEN_SENSOR_MATRIX:
+			wl_token_sensor_matrix_receive(source, packet, length);
+			break;
 		case WL_TOKEN_BOM_ON:
 			//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;
@@ -268,73 +336,190 @@
 }
 
 /**
- * Causes the robot to join an existing token ring, or create one
- * if no token ring exists. The token ring uses global and robot to robot
- * packets, and does not rely on any PAN.
+ * Returns the BOM reading robot source has for robot dest.
+ *
+ * @param source the robot that made the BOM reading
+ * @param dest the robot whose relative location is returned
+ *
+ * @return a BOM reading from robot source to robot dest,
+ * in the range 0-15, or -1 if it is unknown
  **/
-void wl_token_ring_join()
+int wl_token_get_sensor_reading(int source, int dest)
 {
-	WL_DEBUG_PRINT("Joining the token ring.\r\n");
-	ringState = JOINING;
-	joinDelay = JOIN_DELAY;
-	wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN,
-		NULL, 0, 0);
+	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(source, dest);
+	}
+
+	return -1;
 }
 
 /**
- * Causes the robot to leave the token ring. The robot stops
- * alerting others of its location, but continues storing the
- * locations of other robots.
+ * Returns the BOM reading we have for robot dest.
+ *
+ * @param dest the robot whose relative location is returned
+ *
+ * @return a BOM reading from us to robot dest, in the range
+ * 0-15, or -1 if it is unkown
  **/
-void wl_token_ring_leave()
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
+int wl_token_get_my_sensor_reading(int dest)
 {
-	ringState = LEAVING;
+	return wl_token_get_sensor_reading(wl_get_xbee_id(), dest);
 }
 
+
 /**
- * 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.
+ * Returns the number of robots in the token ring.
  *
- * @param robot the ID of the robot which should flash its BOM
+ * @return the number of robots in the token ring
  **/
-void wl_token_request(int robot)
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
+int wl_token_get_robots_in_ring(void)
 {
-	char buf[1];
-	buf[0] = robot;
-	wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_INTERRUPT_REQUEST,
-		buf, 1, 0);
+	return sensor_matrix_get_joined();
 }
 
 /**
- * Returns the BOM reading robot source has for robot dest.
+ * Returns true if the specified robot is in the token ring, false
+ * otherwise.
  *
- * @param source the robot that made the BOM reading
- * @param dest the robot whose relative location is returned
+ * @param robot the robot to check for whether it is in the token ring
+ * @return nonzero if the robot is in the token ring, zero otherwise
+ **/
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
+int wl_token_is_robot_in_ring(int robot)
+{
+	return sensor_matrix_get_in_ring(robot);
+}
+
+/**
+ * Begins iterating through the robots in the token ring.
  *
- * @return a BOM reading from robot source to robot dest,
- * in the range 0-15, or -1 if it is unknown
+ * @see wl_token_iterator_has_next, wl_token_iterator_next
  **/
-int wl_token_get_sensor_reading(int source, int dest)
+void wl_token_iterator_begin(void)
 {
-	return sensor_matrix_get_reading(sensorMatrix, source, dest);
+	int i = 0;
+  //TODO: the compiler may or may not optimize this such that my comment is useless:
+  // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
+  // the overhead of a function call each iteration, call it only once before the loop and store
+  // the value in a variable and check against that variable in the loop condition
+	while (!sensor_matrix_get_in_ring(i) && i < sensor_matrix_get_size()) {
+		i++;
+	}
+
+  //TODO: if you do the above comment, then you can compare this to the variable also
+	if (i == sensor_matrix_get_size()) {
+		i = -1;
+	}
+
+	iteratorCount = i;
 }
 
 /**
- * Returns the BOM reading we have for robot dest.
- * 
- * @param dest the robot whose relative location is returned
+ * Returns true if there are more robots in the token ring
+ * to iterate through, and false otherwise.
  *
- * @return a BOM reading from us to robot dest, in the range
- * 0-15, or -1 if it is unkown
+ * @return nonzero if there are more robots to iterate through,
+ * zero otherwise
+ *
+ * @see wl_token_iterator_begin, wl_token_iterator_next
  **/
-int wl_token_get_my_sensor_reading(int dest)
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
+int wl_token_iterator_has_next(void)
 {
-	return wl_token_get_sensor_reading(wl_get_xbee_id(), dest);
+	return iteratorCount != -1;
 }
 
 /**
+ * Returns the next robot ID in the token ring.
+ *
+ * @return the next robot ID in the token ring, or -1 if none exists
+ *
+ * @see wl_token_iterator_begin, wl_token_iterator_has_next
+ **/
+int wl_token_iterator_next(void)
+{
+	int result = iteratorCount;
+	if (result < 0) {
+		return result;
+	}
+
+  //TODO: the compiler may or may not optimize this such that my comment is useless:
+  // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
+  // the overhead of a function call each iteration, call it only once before the loop and store
+  // the value in a variable and check against that variable in the loop condition
+	iteratorCount++;
+	while (!sensor_matrix_get_in_ring(iteratorCount)
+		&& iteratorCount < sensor_matrix_get_size()) {
+		iteratorCount++;
+	}
+
+  //TODO: if you do the above comment, then you can compare this to the variable also
+	if (iteratorCount == sensor_matrix_get_size()) {
+		iteratorCount = -1;
+	}
+
+	return result;
+}
+
+/**
+ * Returns the number of robots currently in the token ring.
+ *
+ * @return the number of robots in the token ring
+ **/
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
+int wl_token_get_num_robots(void)
+{
+	return sensor_matrix_get_joined();
+}
+
+/**
+ * Returns the number of robots in the sensor matrix.
+ *
+ * @return the number of robots in the sensor matrix
+ **/
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
+int wl_token_get_matrix_size(void)
+{
+	return sensor_matrix_get_size();
+}
+
+/**
  * This method is called when we receive a token pass packet.
+ *
+ * @param source the robot who passed the token to us.
+ **/
+static void wl_token_pass_receive(int source)
+{
+	WL_DEBUG_PRINT("Received token from ");
+	WL_DEBUG_PRINT_INT(source);
+	WL_DEBUG_PRINT(", expected ");
+	WL_DEBUG_PRINT_INT(wl_token_next_robot);
+	WL_DEBUG_PRINT(".\n");
+	// 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 && wl_get_xbee_id() != 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;
+	sensor_matrix_set_in_ring(source, 1);
+	wl_token_get_token();
+}
+
+/**
+ * This method is called when we receive a token pass packet.
  * @param source is the robot it came from
  * @param nextRobot is the robot the token was passed to
  * @param sensorData a char with an id followed by a char with the sensor
@@ -341,24 +526,28 @@
  *		reading for that robot, repeated for sensorDataLength bytes
  * @param sensorDataLength the length in bytes of sensorData
  */
-void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength)
+static void wl_token_sensor_matrix_receive(int source, unsigned char* sensorData, int sensorDataLength)
 {
 	int i, j;
+	char nextRobot;
+
+	bom_on_count = -1;
 	deathDelay = -1;
+	sensor_matrix_set_in_ring(source, 1);
 
-	WL_DEBUG_PRINT("Received the token, next robot is ");
-	WL_DEBUG_PRINT_INT((int)nextRobot);
-	WL_DEBUG_PRINT(" \r\n");
-	sensor_matrix_set_in_ring(sensorMatrix, source, 1);
-
 	//with this packet, we are passed the id of the next robot in the ring
 	//and the sensor matrix, a list of id and sensor reading pairs (two bytes for both)
 	j = 0;
-	for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++)
+  //TODO: the compiler may or may not optimize this such that my comment is useless:
+  // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
+  // the overhead of a function call each iteration, call it only once before the loop and store
+  // the value in a variable and check against that variable in the loop condition
+	for (i = 0; i < sensor_matrix_get_size(); i++)
 	{
-		if (i == source)
+		if (i == source) {
 			continue;
-		
+		}
+
 		//set the sensor information we receive
 		if (j < sensorDataLength / 2 && sensorData[2 * j] == i)
 		{
@@ -368,14 +557,24 @@
 				accepted = -1;
 				WL_DEBUG_PRINT("Someone accepted the robot we did.\r\n");
 			}
-			sensor_matrix_set_reading(sensorMatrix, source, i,
+			sensor_matrix_set_reading(source, i,
 						sensorData[2 * j + 1]);
-			sensor_matrix_set_in_ring(sensorMatrix, i, 1);
+			if (!sensor_matrix_get_in_ring(i))
+			{
+				WL_DEBUG_PRINT("Robot ");
+				WL_DEBUG_PRINT_INT(i);
+				WL_DEBUG_PRINT(" has been added to the sensor matrix of robot ");
+				WL_DEBUG_PRINT_INT(wl_get_xbee_id());
+				WL_DEBUG_PRINT(" due to a packet from robot ");
+				WL_DEBUG_PRINT_INT(source);
+				WL_DEBUG_PRINT(".\r\n");
+			}
+			sensor_matrix_set_in_ring(i, 1);
 			j++;
 		}
 		else
 		{
-			if (sensor_matrix_get_in_ring(sensorMatrix, i))
+			if (sensor_matrix_get_in_ring(i))
 			{
 				WL_DEBUG_PRINT("Robot ");
 				WL_DEBUG_PRINT_INT(i);
@@ -384,7 +583,7 @@
 				WL_DEBUG_PRINT(" due to a packet from robot ");
 				WL_DEBUG_PRINT_INT(source);
 				WL_DEBUG_PRINT(".\r\n");
-				sensor_matrix_set_in_ring(sensorMatrix, i, 0);
+				sensor_matrix_set_in_ring(i, 0);
 			}
 
 			if (i == wl_get_xbee_id() && ringState == MEMBER)
@@ -391,16 +590,15 @@
 			{
 				ringState = NONMEMBER;
 				wl_token_ring_join();
-				
+
 				WL_DEBUG_PRINT("We have been removed from the ring ");
 				WL_DEBUG_PRINT("and are rejoining.\r\n");
 			}
-			
+
 			//the person who accepted us is dead... let's ask again
 			if (i == acceptor)
 			{
-				sensor_matrix_set_in_ring(sensorMatrix,
-						wl_get_xbee_id(), 1);
+				sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
 				ringState = NONMEMBER;
 				acceptor = -1;
 				wl_token_ring_join();
@@ -407,14 +605,31 @@
 			}
 		}
 	}
+	
+	// get the next robot in the token ring
+	i = source + 1;
+	while (1)
+	{
+		if (i == sensor_matrix_get_size()) {
+			i = 0;
+		}
 
-	wl_token_next_robot = nextRobot;
-	
+		if (sensor_matrix_get_in_ring(i) || i == source)
+		{
+			nextRobot = (char)i;
+			break;
+		}
+
+		i++;
+	}
+
+	if (nextRobot != wl_get_xbee_id())
+		wl_token_next_robot = nextRobot;
+
 	deathDelay = get_token_distance(wl_get_xbee_id(), nextRobot) * DEATH_DELAY;
-	
-	//we have the token
-	if (wl_token_next_robot == wl_get_xbee_id())
-		wl_token_get_token();
+
+	if (sensor_matrix_get_joined() == 0 && ringState == JOINING)
+		wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, NULL, 0, nextRobot, WL_TOKEN_PASS_FRAME);
 }
 
 /**
@@ -426,17 +641,21 @@
  * @return the number of passes before the token is expected
  * to reach robot2 from robot1
  **/
-int get_token_distance(int robot1, int robot2)
+static int get_token_distance(int robot1, int robot2)
 {
 	int curr = robot1 + 1;
 	int count = 1;
 	while (1)
 	{
-		if (curr == sensor_matrix_get_size(sensorMatrix))
+    //TODO: the compiler may or may not optimize this such that my comment is useless:
+    // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
+    // the overhead of a function call each iteration, call it only once before the loop and store
+    // the value in a variable and check against that variable in the loop condition
+		if (curr == sensor_matrix_get_size())
 			curr = 0;
 		if (curr == robot2)
 			break;
-		if (sensor_matrix_get_in_ring(sensorMatrix, curr))
+		if (sensor_matrix_get_in_ring(curr))
 			count++;
 		curr++;
 	}
@@ -446,21 +665,29 @@
 /**
  * Passes the token to the next robot in the token ring.
  **/
-void wl_token_pass_token()
+static int wl_token_pass_token()
 {
-	char nextRobot;
+	char nextRobot = 0xFF;
 	int i = wl_get_xbee_id() + 1;
+	char buf[2 * sensor_matrix_get_size()];
 	if (accepted == -1)
 	{
 		while (1)
 		{
-			if (i == sensor_matrix_get_size(sensorMatrix))
+      //TODO: the compiler may or may not optimize this such that my comment is useless:
+      // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
+      // the overhead of a function call each iteration, call it only once before the loop and store
+      // the value in a variable and check against that variable in the loop condition
+			if (i == sensor_matrix_get_size()) {
 				i = 0;
-			if (sensor_matrix_get_in_ring(sensorMatrix, i))
+			}
+
+			if (sensor_matrix_get_in_ring(i))
 			{
 				nextRobot = (char)i;
 				break;
 			}
+
 			i++;
 		}
 	}
@@ -468,40 +695,38 @@
 	{
 		WL_DEBUG_PRINT("Accepting new robot, sending it the token.\r\n");
 		//add a new robot to the token ring
-		sensor_matrix_set_in_ring(sensorMatrix, accepted, 1);
+		sensor_matrix_set_in_ring(accepted, 1);
 		nextRobot = accepted;
 		accepted = -1;
 	}
 
-	//we don't include ourself
-	int packetSize = 1 + 2 * (sensor_matrix_get_joined(sensorMatrix) - 1);
-	char* buf = (char*)malloc(packetSize * sizeof(char));
-	if (!buf)
-	{
-		WL_DEBUG_PRINT_INT(packetSize);
-		WL_DEBUG_PRINT("Out of memory - pass token.\r\n");
-		return;
-	}
-	buf[0] = nextRobot;
-
 	int 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())
+  //TODO: the compiler may or may not optimize this such that my comment is useless:
+  // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
+  // the overhead of a function call each iteration, call it only once before the loop and store
+  // the value in a variable and check against that variable in the loop condition
+	for (i = 0; i < sensor_matrix_get_size(); i++) {
+		if (sensor_matrix_get_in_ring(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);
+			buf[2*j] = i;
+			buf[2*j + 1] = sensor_matrix_get_reading(wl_get_xbee_id(), i);
 			j++;
 		}
-	
+	}
+
+	int packetSize = 2 * j * sizeof(char);
 	WL_DEBUG_PRINT("Passing the token to robot ");
-	WL_DEBUG_PRINT_INT(buf[0]);
+	WL_DEBUG_PRINT_INT(nextRobot);
 	WL_DEBUG_PRINT(".\r\n");
-	wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS,
-		buf, packetSize, 3);
+	if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_SENSOR_MATRIX, buf, packetSize, 0) != 0)
+		return -1;
+	if (wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, NULL, 0, nextRobot, WL_TOKEN_PASS_FRAME))
+		return -1;
 
 	wl_token_next_robot = nextRobot;
 	deathDelay = DEATH_DELAY;
-	free(buf);
+
+	return 0;
 }
 
 /**
@@ -511,12 +736,15 @@
  *
  * @param source the robot whose BOM is on
  **/
-void wl_token_bom_on_receive(int source)
+static void wl_token_bom_on_receive(int source)
 {
 	WL_DEBUG_PRINT("Robot ");
 	WL_DEBUG_PRINT_INT(source);
 	WL_DEBUG_PRINT(" has flashed its bom.\r\n");
-	sensor_matrix_set_reading(sensorMatrix, wl_get_xbee_id(), 
+
+	bom_on_count = 0;
+
+	sensor_matrix_set_reading(wl_get_xbee_id(),
 		source, get_max_bom_function());
 }
 
@@ -524,23 +752,23 @@
  * This method is called when we receive the token. Upon receiving
  * the token, we must send a BOM_ON packet, flash the BOM, and send
  * the token to the next robot.
- * 
+ *
  * If there is a pending request for the token, this is processed first.
  **/
-void wl_token_get_token()
+static void wl_token_get_token()
 {
 	WL_DEBUG_PRINT("We have the token.\r\n");
 	if (ringState == ACCEPTED)
 	{
-		sensor_matrix_set_in_ring(sensorMatrix,
-			wl_get_xbee_id(), 1);
+		sensor_matrix_set_in_ring(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)
 	{
-		sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);
+		sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
 		if (ringState == NONMEMBER)
 		{
 			WL_DEBUG_PRINT("We should have left the token ring, but didn't.\r\n");
@@ -547,27 +775,9 @@
 		}
 		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);
+	wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON, NULL, 0, 0);
 
 	bom_on_function();
 	#ifdef ROBOT
@@ -574,12 +784,13 @@
 	delay_ms(BOM_DELAY);
 	#endif
 	bom_off_function();
-	
-	if (!sensor_matrix_get_in_ring(sensorMatrix, wl_get_xbee_id()))
+
+	if (!sensor_matrix_get_in_ring(wl_get_xbee_id()))
 	{
 		WL_DEBUG_PRINT("Removed from sensor matrix while flashing BOM.\r\n");
+		return;
 	}
-	
+
 	wl_token_pass_token();
 }
 
@@ -591,7 +802,7 @@
  *
  * @param source the robot who requested to join
  **/
-void wl_token_join_receive(int source)
+static void wl_token_join_receive(int source)
 {
 	WL_DEBUG_PRINT("Received joining request from robot ");
 	WL_DEBUG_PRINT_INT(source);
@@ -606,20 +817,22 @@
 	//we can only accept one request at a time
 	if (accepted != -1)
 		return;
-	
+
 	//check if we are the preceding robot in the token ring
 	int i = source - 1;
 	while (1)
 	{
 		if (i < 0)
-			i = sensor_matrix_get_size(sensorMatrix) - 1;
+			i = sensor_matrix_get_size() - 1;
+
 		//we must send a join acceptance
 		if (i == wl_get_xbee_id())
 			break;
 
 		//another robot will handle it
-		if (sensor_matrix_get_in_ring(sensorMatrix, i))
+		if (sensor_matrix_get_in_ring(i))
 			return;
+
 		i--;
 	}
 
@@ -626,15 +839,13 @@
 	accepted = source;
 	wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN_ACCEPT,
 		NULL, 0, source, TOKEN_JOIN_ACCEPT_FRAME);
-	
+
 	WL_DEBUG_PRINT("Accepting robot ");
 	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)
+	if (sensor_matrix_get_joined() == 1)
 		wl_token_pass_token();
 }
 
@@ -645,169 +856,15 @@
  *
  * @param source the robot who accepted us
  **/
-void wl_token_join_accept_receive(int source)
+static void wl_token_join_accept_receive(int source)
 {
 	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;
 
 	//add ourselves to the token ring
-	sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);
+	sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
 }
-
-/**
- * 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
- **/
-int wl_token_get_robots_in_ring(void)
-{
-	return sensor_matrix_get_joined(sensorMatrix);
-}
-
-/**
- * Returns true if the specified robot is in the token ring, false
- * otherwise.
- *
- * @param robot the robot to check for whether it is in the token ring
- * @return nonzero if the robot is in the token ring, zero otherwise
- **/
-int wl_token_is_robot_in_ring(int robot)
-{
-	return sensor_matrix_get_in_ring(sensorMatrix, robot);
-}
-
-/**
- * Begins iterating through the robots in the token ring.
- *
- * @see wl_token_iterator_has_next, wl_token_iterator_next
- **/
-void wl_token_iterator_begin(void)
-{
-	int i = 0;
-	iteratorCount = 0;
-	while (!sensor_matrix_get_in_ring(sensorMatrix, i) &&
-			i < sensor_matrix_get_size(sensorMatrix))
-		i++;
-	if (i == sensor_matrix_get_size(sensorMatrix))
-		i = -1;
-}
-
-/**
- * Returns true if there are more robots in the token ring
- * to iterate through, and false otherwise.
- *
- * @return nonzero if there are more robots to iterate through,
- * zero otherwise
- *
- * @see wl_token_iterator_begin, wl_token_iterator_next
- **/
-int wl_token_iterator_has_next(void)
-{
-	return iteratorCount == -1;
-}
-
-/**
- * Returns the next robot ID in the token ring.
- *
- * @return the next robot ID in the token ring, or -1 if none exists
- *
- * @see wl_token_iterator_begin, wl_token_iterator_has_next
- **/
-int wl_token_iterator_next(void)
-{
-	int result = iteratorCount;
-	if (result < 0)
-		return result;
-
-	iteratorCount++;
-	while (!sensor_matrix_get_in_ring(sensorMatrix, iteratorCount) &&
-		iteratorCount < sensor_matrix_get_size(sensorMatrix))
-		iteratorCount++;
-	if (iteratorCount == sensor_matrix_get_size(sensorMatrix))
-		iteratorCount = -1;
-	return result;
-}
-
-/**
- * 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.
- *
- * @param source the robot requesting interruption
- * @param robt the robot requested to interrupt the token ring
- **/
-void wl_token_interrupt_request_receive(int source, int robot)
-{
-	queue_add(interrupting, (void*)robot);
-}
-
-int wl_token_get_num_robots(void){
-  return sensor_matrix_get_joined(sensorMatrix);
-}
-
-int wl_token_get_matrix_size(void){
-  return sensor_matrix_get_size(sensorMatrix);
-}
Index: branches/encoders/code/projects/libwireless/lib/Doxyfile
===================================================================
--- branches/encoders/code/projects/libwireless/lib/Doxyfile	(revision 1344)
+++ branches/encoders/code/projects/libwireless/lib/Doxyfile	(revision 1345)
@@ -1,5 +1,5 @@
 # Doxyfile 1.4.6
- 
+               
 # This file describes the settings to be used by the documentation system
 # doxygen (www.doxygen.org) for a project
 #
@@ -31,7 +31,7 @@
 # where doxygen was started. If left blank the current directory will be used.
 
 OUTPUT_DIRECTORY       = /home/rc99/trac/docs/libwireless
-
+     
 # If the CREATE_SUBDIRS tag is set to YES, then doxygen will create 
 # 4096 sub-directories (in 2 levels) under the output directory of each output 
 # format and will distribute the generated files over these directories. 
Index: branches/encoders/code/projects/libwireless/lib/sensor_matrix.h
===================================================================
--- branches/encoders/code/projects/libwireless/lib/sensor_matrix.h	(revision 1344)
+++ branches/encoders/code/projects/libwireless/lib/sensor_matrix.h	(revision 1345)
@@ -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
@@ -17,53 +46,56 @@
  * @{
  **/
 
+#define MAXIMUM_XBEE_ID		0x10
+#define READING_UNKNOWN		0xFF
+
 /**
  * @struct SensorMatrix
  *
  * A sensor matrix.
  **/
+//TODO: the order of member variables in this struct should be changed in case the compile packs the struct
+// In order to achieve the best packing, the variables should be listed in order of decreasing memory size.
+// Thus, pointers should be first, followed by int, followed by char.
 typedef struct
 {
 	/**
-	 * The size of the sensor matrix.
-	**/
-	unsigned int size;
-	/**
-	 * The matrix. Each row represents the readings of one
-	 * robot.
+	 * The number of robots in the token ring.
 	 **/
-	int** matrix;
+	int numJoined;
 	/**
 	 * The element representing a robot is true if that robot
 	 * is in the token ring and false otherwise.
 	 **/
-	int* joined;
+	unsigned char joined[MAXIMUM_XBEE_ID];
+
+	// on the bayboard, we don't include the matrix to save memory.
+#ifndef BAYBOARD
 	/**
-	 * The number of robots in the token ring.
+	 * The matrix. Each row represents the readings of one
+	 * robot.
 	 **/
-	int numJoined;
+	unsigned char matrix[MAXIMUM_XBEE_ID][MAXIMUM_XBEE_ID];
+#endif
 } SensorMatrix;
 
 /**@brief Create a sensor matrix **/
-SensorMatrix* sensor_matrix_create(void);
-/**@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);
-/**@brief Remove a robot from a sensor matrix **/
-void sensor_matrix_remove_robot(SensorMatrix* m, unsigned int id);
+void sensor_matrix_create(void);
 /**@brief Set a reading in a sensor matrix **/
-void sensor_matrix_set_reading(SensorMatrix* m, int observer, int robot, int reading);
+void sensor_matrix_set_reading(int observer, int robot, int reading);
 /**@brief Get a reading in a sensor matrix **/
-int sensor_matrix_get_reading(SensorMatrix* m, int observer, int robot);
+int sensor_matrix_get_reading(int observer, int robot);
 /**@brief Set whether the robot is in the token ring **/
-void sensor_matrix_set_in_ring(SensorMatrix* m, int robot, int in);
+void sensor_matrix_set_in_ring(int robot, int in);
 /**@brief Get whether the robot is in the sensor ring **/
-int sensor_matrix_get_in_ring(SensorMatrix* m, int robot);
-/**@brief Get the size of the sensor matrix **/
-int sensor_matrix_get_size(SensorMatrix* m);
+int sensor_matrix_get_in_ring(int robot);
 /**@brief Get the number of robots which have joined the token ring **/
-int sensor_matrix_get_joined(SensorMatrix* m);
+int sensor_matrix_get_joined(void);
+/**@brief Get the maximum size of the sensor matrix **/
+int sensor_matrix_get_size(void);
 
 /** @} **/ //end defgroup
 
+
+#endif
+
Index: branches/encoders/code/projects/libwireless/lib/wireless.h
===================================================================
--- branches/encoders/code/projects/libwireless/lib/wireless.h	(revision 1344)
+++ branches/encoders/code/projects/libwireless/lib/wireless.h	(revision 1345)
@@ -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,12 +31,19 @@
  *
  * @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.
 /**
  * The maximum number of packet groups.
  **/
+//TODO: a PacketGroupHandler is at least 10 bytes (I don't know if function pointers are 2 bytes
+// or 4 bytes).  That means that in the c file, your array of packet groups is at least 160 bytes.
+// Normally that might be fine (the robot's avr chips have 4k SRAM), but austin's chip only has
+// 1k SRAM, so if this number can be reduced or if the size of the struct could be reduced, that would be a plus.
 #define WL_MAX_PACKET_GROUPS 16
 
 /**
@@ -20,12 +52,12 @@
  *
  * Contains functions and definitions for dealing with wireless functionality.<br><br>
  *
- * The wireless library provides a modular method for dealing with 
+ * The wireless library provides a modular method for dealing with
  * wireless packets, by allowing packet groups to be registered.
  * A packet group is a collection of packets which share a packet
- * group code. Each packet in the group also has a type. A packet 
+ * group code. Each packet in the group also has a type. A packet
  * group code and type are sent with each packet. When a packet
- * with a group code registered in the wireless library is 
+ * with a group code registered in the wireless library is
  * received, the corresponding event handler is called. The
  * event handler uses the packet type and other information
  * stored in the packet to respond.<br><br>
@@ -39,11 +71,14 @@
 
 /**
  * @struct PacketGroupHandler
- * A PacketGroupHandler represents a packet group, and is used to 
+ * A PacketGroupHandler represents a packet group, and is used to
  * register a packet group with the wireless library. It contains
  * handlers for various events which can occur related to a packet
  * group.
  **/
+//TODO: the order of member variables in this struct should be changed in case the compile packs the struct
+// In order to achieve the best packing, the variables should be listed in order of decreasing memory size.
+// Thus, pointers should be first, followed by int, followed by char.
 typedef struct
 {
 	/**
@@ -51,6 +86,8 @@
 	 * must be unique. The maximum number of packet groups
 	 * is defined by WL_MAX_PACKET_GROUPS.
 	 **/
+  //TODO: if this number must be less than or equal to WL_MAX_PACKET_GROUPS, don't you only need
+  // one byte for it and it can be made an unsigned char?
 	unsigned int groupCode;
 
 	/**
@@ -58,7 +95,7 @@
 	 * but in wl_do).
 	 **/
 	void (*timeout_handler) (void);
-	
+
 	/**
 	 * Called when a transmit status packet is received
 	 * from the XBee where the first four bits of the frame
@@ -69,7 +106,7 @@
 	 * we did not.
 	 **/
 	void (*handle_response) (int frame, int received);
-	
+
 	/**
 	 * Called when we receive a packet from this group.
 	 *
@@ -79,18 +116,17 @@
 	 * @param packet the packet received
 	 * @param length the length of the packet
 	 **/
-	void (*handle_receive) (char type, int source, unsigned char* packet,
-							int length);
-	
+	void (*handle_receive) (char type, int source, unsigned char* packet, int length);
+
 	/**
 	 * Called for any cleanup when the network is turned off.
 	 **/
 	void (*unregister) (void);
-	
+
 } 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 **/
@@ -101,28 +137,28 @@
 void wl_unregister_packet_group(PacketGroupHandler* h);
 
 /**@brief Send a packet to a specific robot in any PAN **/
-void wl_send_robot_to_robot_global_packet(char group, char type,
-		char* data, int len, int dest, char frame);
+int wl_send_robot_to_robot_global_packet(char group, char type, char* data, int len, int dest, char frame);
 /**@brief Send a packet to a specific robot in our PAN **/
-void wl_send_robot_to_robot_packet(char group, char type,
-		char* data, int len, int dest, char frame);
+int wl_send_robot_to_robot_packet(char group, char type, char* data, int len, int dest, char frame);
 /**@brief Send a packet to all robots **/
-void wl_send_global_packet(char group, char type,
-		char* data, int len, char frame);
+int wl_send_global_packet(char group, char type, char* data, int len, char frame);
 /**@brief Send a packet to all robots in our PAN **/
-void wl_send_pan_packet(char group, char type,
-		char* data, int len, char frame);
+void wl_send_pan_packet(char group, char type, char* data, int len, char frame);
 
 /**@brief Set the PAN we are using **/
-void wl_set_pan(int pan);
+int wl_set_pan(int pan);
 /**@brief Get the PAN we are using **/
 int wl_get_pan(void);
 /**@brief Set the channel we are using **/
-void wl_set_channel(int channel);
+int wl_set_channel(int channel);
 /**@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/encoders/code/projects/libwireless/lib/xbee.c
===================================================================
--- branches/encoders/code/projects/libwireless/lib/xbee.c	(revision 1344)
+++ branches/encoders/code/projects/libwireless/lib/xbee.c	(revision 1345)
@@ -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,9 +54,8 @@
 #include <stdlib.h>
 #include <string.h>
 
-#include <queue.h>
-
 #define XBEE_FRAME_START 0x7E
+#define XBEE_GET_PACKET_TIMEOUT 1000
 
 /*Frame Types*/
 #define XBEE_FRAME_STATUS 0x8A
@@ -36,53 +70,62 @@
 /*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_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_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
-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
 
-Queue* xbee_queue;
+// 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
-char xbee_buf[128];
-int currentBufPos = 0;
+static char xbee_buf[PACKET_BUFFER_SIZE];
+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;
 
 /*Function Implementations*/
 
@@ -90,36 +133,69 @@
 
 /**
  * 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("\nOut 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
 
 #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);
-		queue_add(xbee_queue, (void*)(int)c);
+		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 0;
+
+	return NULL;
 }
 
 #endif
@@ -126,52 +202,134 @@
 
 /**
  * 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()
 {
-	xbee_queue = queue_create();
-	
-	#ifdef ROBOT
+	WL_DEBUG_PRINT("in xbee_init\n");
+#ifdef ROBOT
 
 	//enable the receiving interrupt
-	#ifdef FIREFLY
+#ifdef FIREFLY
 	UCSR0B |= _BV(RXCIE) | _BV(RXEN);
-	#else
+#else
+#ifdef BAYBOARD
+	UCSR1B |= _BV(RXCIE1);
+#else
 	UCSR1B |= _BV(RXCIE);
-	#endif
+#endif
+#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)
+#else
+	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);
+		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");
 	}
-	lockf(xbee_stream, F_LOCK, 0);
-	
-	xbee_listen_thread =
-		(pthread_t*)malloc(sizeof(pthread_t));
-	
-	int ret = pthread_create(xbee_listen_thread, NULL, 
-		listen_to_xbee, NULL);
+
+	// 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)
 	{
-		printf("Failed to create listener thread.\r\n");
-		exit(0);
+		WL_DEBUG_PRINT("Failed to create listener thread.\n");
+		return -1;
 	}
-	#endif
-	xbee_enter_command_mode();
-	xbee_enter_api_mode();
-	xbee_exit_command_mode();
-	xbee_send_read_at_command("MY");
+#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
-	while (xbee_address == 0) xbee_get_packet(NULL);
+  //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
 }
 
 /**
@@ -186,26 +344,30 @@
 	lockf(xbee_stream, F_ULOCK, 0);
 	close(xbee_stream);
 	#endif
-	queue_destroy(xbee_queue);
 }
 
 /**
  * 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
+
 	int ret = write(xbee_stream, buf, size);
 	//success
 	if (ret == size)
-		return;
+		return 0;
 	if (ret == -1)
 	{
 		//interrupted by system signal, probably timer interrupt.
@@ -212,16 +374,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;
+		WL_DEBUG_PRINT("Failed to write to xbee\r\n");
+		return -1;
 	}
 
 	//write was interrupted after writing ret bytes
-	xbee_send(buf + ret, size - ret);
-	#endif
+	return xbee_send(buf + ret, size - ret);
+#endif
 }
 
 /**
@@ -229,16 +390,22 @@
  *
  * @param c the string to send to the XBEE
  **/
-void xbee_send_string(char* c)
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
+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)
-		printf("Failed to read from xbee.\r\n");
+	if (read(xbee_stream, buf, size) == -1) {
+		WL_DEBUG_PRINT("Failed to read from xbee.\r\n");
+		return -1;
+	}
+
+	return 0;
 }
 #endif
 
@@ -245,36 +412,53 @@
 /**
  * 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;
+	}
+
+	if (xbee_wait_for_ok() != 0) {
+	  return -1;
+	}
+	  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;
 }
 
 /**
  * Wait until the string "OK\r" is received from the XBee.
  **/
-void xbee_wait_for_ok()
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
+static int xbee_wait_for_ok()
 {
-	xbee_wait_for_string("OK\r", 3);
+	return xbee_wait_for_string("OK\r", 3);
 }
 
 /**
@@ -284,19 +468,35 @@
  * @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)
 {
 	char* curr = s;
-	while (curr - s < len)
-	{
-		if (queue_is_empty(xbee_queue))
-			continue;
-		char c = (char)(int)queue_remove(xbee_queue);
-		if (c == *curr)
-			curr++;
-		else
-			curr = s;
+	while (curr - s < len) {
+		// 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;
 }
 
 /**
@@ -348,7 +548,7 @@
  * @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)
 {
 	char prefix[3];
 	prefix[0] = XBEE_FRAME_START;
@@ -355,9 +555,20 @@
 	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;
 }
 
 /**
@@ -367,9 +578,11 @@
  * 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)
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
+static int xbee_send_read_at_command(char* command)
 {
-	xbee_send_modify_at_command(command, NULL);
+	return xbee_send_modify_at_command(command, NULL);
 }
 
 /**
@@ -379,11 +592,11 @@
  * @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)
 {
 	char buf[16];
 	int i;
-	
+
 	buf[0] = XBEE_FRAME_AT_COMMAND;
 	buf[1] = 1;
 	buf[2] = command[0];
@@ -395,26 +608,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
@@ -422,8 +638,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];
@@ -433,7 +648,7 @@
 	if (len > 100)
 	{
 		WL_DEBUG_PRINT("Packet is too large.\r\n");
-		return;
+		return -1;
 	}
 
 	//data for sending request
@@ -453,10 +668,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;
 }
 
 /**
@@ -471,7 +700,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>
@@ -478,7 +707,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>
@@ -488,7 +717,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
@@ -495,33 +724,58 @@
  **/
 int xbee_get_packet(unsigned char* dest)
 {
+     int ret;
 	//start reading a packet with XBEE_FRAME_START
 	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++;
 	}
 
 	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)
+		{
 			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;
-	
+
 	if (!xbee_verify_checksum(xbee_buf, len + 4))
 	{
 		WL_DEBUG_PRINT("XBee checksum failed.\r\n");
@@ -529,15 +783,20 @@
 	}
 
 	//we will take care of the packet
-	if (xbee_handle_packet(xbee_buf + 3, len))
-		return -1;
 	
-	if (dest == NULL)
+	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++)
+	for (i = 3; i < len + 3; i++) {
 		dest[i - 3] = xbee_buf[i];
+	}
 	return len;
 }
 
@@ -581,8 +840,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)
 	{
@@ -593,7 +851,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;
@@ -611,13 +869,14 @@
 		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++)
+		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);
@@ -626,7 +885,9 @@
 		if (xbee_address == 0)
 		{
 			WL_DEBUG_PRINT("XBee 16-bit address must be set using ATMY.\r\n");
+			#ifndef ROBOT
 			exit(0);
+			#endif
 		}
 	}
 }
@@ -640,11 +901,12 @@
  *
  * @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)
 {
+
 	char command[3] = {1, 2, 3};
 	if (len <= 0) //this should not happend
 	{
@@ -651,7 +913,7 @@
 		WL_DEBUG_PRINT("Non-positive packet length.\r\n");
 		return 0;
 	}
-	
+
 	switch ((unsigned char)packet[0]) //packet type
 	{
 		case XBEE_FRAME_STATUS:
@@ -661,8 +923,7 @@
 			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;
@@ -673,7 +934,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;
@@ -680,15 +941,17 @@
 	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.
  **/
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
 unsigned int xbee_get_pan_id()
 {
 	return xbee_panID;
@@ -697,23 +960,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);
 }
 
 /**
@@ -723,6 +988,8 @@
  *
  * @see xbee_set_channel
  **/
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
 int xbee_get_channel(void)
 {
 	return xbee_channel;
@@ -735,8 +1002,16 @@
  *
  * @return the 16-bit address of the XBee.
  **/
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
 unsigned int xbee_get_address()
 {
 	return xbee_address;
 }
 
+#ifndef ROBOT
+void xbee_set_com_port(char* port)
+{
+	xbee_com_port = port;
+}
+#endif
Index: branches/encoders/code/projects/libwireless/lib/wl_token_ring.h
===================================================================
--- branches/encoders/code/projects/libwireless/lib/wl_token_ring.h	(revision 1344)
+++ branches/encoders/code/projects/libwireless/lib/wl_token_ring.h	(revision 1345)
@@ -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
@@ -19,19 +47,17 @@
  **/
 
 /**@brief Register the token ring group with the wireless library.**/
-void wl_token_ring_register(void);
+int wl_token_ring_register(void);
 /**@brief Unregister the token ring group with the wirelss library.**/
 void wl_token_ring_unregister(void);
 /**@brief Set the functions called to turn the bom on and off.**/
-void wl_token_ring_set_bom_functions(void (*on_function) (void), 
-		void (*off_function) (void), int (*max_bom_function) (void));
+void wl_token_ring_set_bom_functions(void (*on_function) (void), void (*off_function) (void),
+  int (*max_bom_function) (void));
 
 /**@brief Join the token ring **/
-void wl_token_ring_join(void);
+int 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/encoders/code/projects/libwireless/lib/wl_error_group.c
===================================================================
--- branches/encoders/code/projects/libwireless/lib/wl_error_group.c	(revision 1344)
+++ branches/encoders/code/projects/libwireless/lib/wl_error_group.c	(revision 1345)
@@ -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>
@@ -27,6 +61,8 @@
  * This function must be called before any other wl_error
  * function.
  **/
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
 void wl_error_register(void)
 {
 	wl_register_packet_group(&wl_error_handler);
@@ -38,6 +74,8 @@
  *
  * @see wl_error_register
  **/
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
 void wl_error_unregister(void)
 {
 	wl_unregister_packet_group(&wl_error_handler);
@@ -77,9 +115,11 @@
 
 /**
  * Send an error message as a string.
- * 
+ *
  * @param str the error message to send
  **/
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
 void wl_error_send_string(char* str)
 {
 	wl_send_global_packet(WL_ERROR_GROUP, WL_ERROR_STRING_TYPE,
Index: branches/encoders/code/projects/libwireless/lib/wl_defs.h
===================================================================
--- branches/encoders/code/projects/libwireless/lib/wl_defs.h	(revision 1344)
+++ branches/encoders/code/projects/libwireless/lib/wl_defs.h	(revision 1345)
@@ -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
 
@@ -15,29 +53,21 @@
 #define WL_TOKEN_RING_GROUP 2
 
 #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_SENSOR_MATRIX 2
+#define WL_TOKEN_BOM_ON 3
+#define WL_TOKEN_JOIN 4
+#define WL_TOKEN_JOIN_ACCEPT 5
 
 // 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
-#define WL_RECHARGE_GROUP 3
-
-#define WL_RECHARGE_POLL_STATIONS 1
-#define WL_RECHARGE_STATION_AVAILABLE 2
-#define WL_RECHARGE_REQUEST 3
-#define WL_RECHARGE_REQUEST_ACCEPT 4
-#define WL_RECHARGE_VERIFY 5
-#define WL_RECHARGE_CANCEL 6
-#define WL_RECHARGE_SEEKING 7
-#define WL_RECHARGE_DOCKED 8
-
 #ifdef WL_DEBUG
 
 #ifdef ROBOT
@@ -63,3 +93,5 @@
 
 #endif
 
+#endif
+
Index: branches/encoders/code/projects/libwireless/lib/xbee.h
===================================================================
--- branches/encoders/code/projects/libwireless/lib/xbee.h	(revision 1344)
+++ branches/encoders/code/projects/libwireless/lib/xbee.h	(revision 1345)
@@ -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,24 +74,26 @@
 #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 **/
 int xbee_get_packet(unsigned char* packet);
 /**@brief Send a packet to the XBee **/
-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);
 /**@brief Set the PAN ID for the XBee **/
-void xbee_set_pan_id(int id);
+int xbee_set_pan_id(int id);
 /**@brief Get the XBee's PAN ID **/
 unsigned int xbee_get_pan_id(void);
 /**@brief Set the channel the XBee is currently using **/
-void xbee_set_channel(int channel);
+int xbee_set_channel(int channel);
 /**@brief Get the channel the XBee is currently using **/
 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/encoders/code/projects/libwireless/lib/Makefile
===================================================================
--- branches/encoders/code/projects/libwireless/lib/Makefile	(revision 1344)
+++ branches/encoders/code/projects/libwireless/lib/Makefile	(revision 1345)
@@ -1,13 +1,14 @@
 ############################
 ### Update this Section ####
 ############################
-
 COLONYROOT = ../../../..
 
 # Target file name (without extension).
 TARGET = libwireless
+COLONET_TARGET = libwireless_colonet
 
 ############################
+# don't touch this unless you know what you're doing.
 
 CDEFS = 
 
@@ -48,7 +49,7 @@
 #     Native formats for AVR-GCC's -g are dwarf-2 [default] or stabs.
 #     AVR Studio 4.10 requires dwarf-2.
 #     AVR [Extended] COFF format requires stabs, plus an avr-objcopy run.
-DEBUG = 
+DEBUG =
 # dwarf-2
 
 # List any extra directories to look for include files here.
@@ -155,7 +156,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.
@@ -165,7 +166,6 @@
 
 publish:
 	$(COPY) $(TARGET).a $(COLONYROOT)/code/lib/bin
-	$(COPY) *.c $(COLONYROOT)/code/lib/src/$(TARGET)
 	$(COPY) *.h $(COLONYROOT)/code/lib/include/$(TARGET)
 
 docs:
@@ -176,9 +176,19 @@
 	$(AR) $(ALL_ASFLAGS) $(TARGET).a $(OBJ)
 
 computer:
-	gcc *.c -g -I. -c
+	gcc *.c -Wall -Wshadow -Wextra -g -I. -c
 	ar rcs $(TARGET).a $(OBJ)
 
+# we should fine a better way to do this.....
+bayboard:
+	avr-gcc *.c -DBAYBOARD -mmcu=atmega164p -I. -DF_CPU=8000000UL -DROBOT -I../../../../code/lib/include/libbayboard \
+			-L../../../../code/lib/bin/bayboard -Wall -Wshadow -Wextra -std=gnu99 -MD -MP -MF -c
+	avr-ar rcs $(TARGET).a $(OBJ)
+
+colonet:
+	g++ -Wall -Wshadow -Wextra *.c -g -I. -c
+	ar rcs $(COLONET_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.
@@ -204,6 +214,7 @@
 	@echo
 	@echo $(MSG_CLEANING)
 	$(REMOVE) $(TARGET).a
+	$(REMOVE) $(COLONET_TARGET).a
 	$(REMOVE) $(OBJ)
 	$(REMOVE) $(LST)
 	$(REMOVE) $(SRC:.c=.s)
Index: branches/encoders/code/projects/libwireless/lib/wl_error_group.h
===================================================================
--- branches/encoders/code/projects/libwireless/lib/wl_error_group.h	(revision 1344)
+++ branches/encoders/code/projects/libwireless/lib/wl_error_group.h	(revision 1345)
@@ -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/encoders/code/projects/libwireless/lib/sensor_matrix.c
===================================================================
--- branches/encoders/code/projects/libwireless/lib/sensor_matrix.c	(revision 1344)
+++ branches/encoders/code/projects/libwireless/lib/sensor_matrix.c	(revision 1345)
@@ -1,267 +1,163 @@
-#include <stdlib.h>
-#include <stdio.h>
-#include <wl_defs.h>
-
-#include "sensor_matrix.h"
-
-#define DEFAULT_SENSOR_MATRIX_SIZE 20
-
-/*Sensor Matrix Functions*/
-void sensor_matrix_expand(SensorMatrix* m, int nextSize);
-
 /**
- * Initializes the sensor matrix.
+ * Copyright (c) 2007 Colony Project
  *
- * @return the newly created sensor matrix
+ * 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.
  **/
-SensorMatrix* sensor_matrix_create()
-{
-	SensorMatrix* m;
-	int i;
-	
-	m = malloc(sizeof(SensorMatrix));
-	if (!m)
-	{
-		WL_DEBUG_PRINT("Out of memory - create sensor matrix.\r\n");
-		return NULL;
-	}
-	m->size = DEFAULT_SENSOR_MATRIX_SIZE;
-	m->matrix = malloc(m->size * sizeof(int*));
-	m->joined = malloc(m->size * sizeof(int));
-	m->numJoined = 0;
-	if (!(m->matrix) || !(m->joined))
-	{
-		WL_DEBUG_PRINT("Out of memory - create sensor matrix 2.\r\n");
-		return NULL;
-	}
-	
-	for (i = 0; i < m->size; i++)
-	{
-		m->matrix[i] = NULL;
-		m->joined[i] = 0;
-	}
-	return m;
-}
 
 /**
- * Deletes and frees memory from the sensor matrix.
+ * @file sensor_matrix.c
+ * @brief Sensor Matrix implementation
  *
- * @param m the sensor matrix to delete
+ * Implementation of a sensor matrix for storing localization implementation.
+ *
+ * @author Brian Coltin, Colony Project, CMU Robotics Club
  **/
-void sensor_matrix_destroy(SensorMatrix* m)
-{
-	int i;
-	for (i = 0; i < m->size; i++)
-		if (m->matrix[i] != NULL)
-			free(m->matrix[i]);
-	free(m->matrix);
-	free(m->joined);
-	free(m);
-}
 
-/**
- * Adds robot with XBee id id to the sensor matrix.
- * 
- * @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)
-{
-	int i;
-	if (id >= m->size)
-		sensor_matrix_expand(m, id + 1);
-	if (m->matrix[id] != NULL)
-		return;
-	
-	m->matrix[id] = malloc(m->size * sizeof(int));
-	if (!(m->matrix[id]))
-	{
-		WL_DEBUG_PRINT("Out of memory - add robot.\r\n");
-		return;
-	}
+#include <stdlib.h>
+#include <stdio.h>
+#include <wl_defs.h>
 
-	for (i = 0; i < m->size; i++)
-		if (m->matrix[i] != NULL)
-			m->matrix[i][id] = -1;
-}
+#include "sensor_matrix.h"
 
-/**
- * Removes robot with id from the sensor matrix, and removes
- * all sensor information regarding the robot.
- *
- * @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)
-{
-	int i;
+// the global sensor matrix
+SensorMatrix m;
 
-	if (id >= m->size || m->matrix[id] == NULL)
-	{
-		WL_DEBUG_PRINT("Removing robot not added to matrix.\r\n");
-		return;
-	}
-	
-	free(m->matrix[id]);
-	m->matrix[id] = NULL;
-	
-	for (i = 0 ; i < m->size; i++)
-		if (m->matrix[i] != NULL)
-			m->matrix[i][id] = -1;
-	
-	m->joined[id] = 0;
-}
-
 /**
- * Expands the size of the sensor matrix if an id number we attempt
- * to add is too large.
+ * Initializes the sensor matrix.
  *
- * @param m the sensor matrix to expand
- * @param size the new size of the sensor matrix
+ * @return the newly created sensor matrix
  **/
-//Note: this has probably not been tested, hopefully it works
-void sensor_matrix_expand(SensorMatrix* m, int nextSize)
+void sensor_matrix_create()
 {
 	int i, j;
-	WL_DEBUG_PRINT("Expanding sensor matrix.\r\n");
-	
-	int** tempMatrix = malloc(nextSize * sizeof(int*));
-	if (!tempMatrix)
-	{
-		WL_DEBUG_PRINT("Out of memory - expand matrix.\r\n");
-		return;
-	}
-	
-	for (i = 0; i < nextSize; i++)
-		tempMatrix[i] = NULL;
-	
-	//copy over old sensor data
-	for (i = 0; i < m->size; i++)
-		if (m->matrix[i] != NULL)
-		{
-			tempMatrix[i] = malloc(nextSize * sizeof(int));
-			if (!tempMatrix[i])
-			{
-				WL_DEBUG_PRINT("Out of memory - expand matrix 2.\r\n");
-				return;
-			}
-			for (j = 0; j < m->size; j++)
-				tempMatrix[i][j] = m->matrix[i][j];
-			for (j = m->size; j < nextSize; j++)
-				tempMatrix[i][j] = -1;
-			
-			free(m->matrix[i]);
-		}
-	
-	free(m->matrix);
-	m->matrix = tempMatrix;
-	m->size = nextSize;
 
-	//expand the size of joined
-	int* tempJoined = malloc(nextSize * sizeof(int));
-	if (!tempJoined)
+	m.numJoined = 0;
+
+	for (i = 0; i < MAXIMUM_XBEE_ID; i++)
 	{
-		WL_DEBUG_PRINT("Out of memory - expand matrix 3.\r\n");
-		return;
+		m.joined[i] = 0;
+#ifndef BAYBOARD
+		for (j = 0; j < MAXIMUM_XBEE_ID; j++)
+			m.matrix[i][j] = READING_UNKNOWN;
+#endif
 	}
-	
-	for (i = 0; i < m->size; i++)
-		tempJoined[i] = m->joined[i];
-	for (i = m->size; i < nextSize; i++)
-		tempJoined[i] = 0;
-	
-	free(m->joined);
-	m->joined = tempJoined;
 }
 
 /**
  * Sets the sensor reading for robot robot to reading.
- * 
- * @param m the sensor matrix to set the reading for
+ *
  * @param observer the id of the robot who made the reading
  * @param robot the id of the robot who the reading is for
  * @param reading the BOM reading from observer to robot
  */
-void sensor_matrix_set_reading(SensorMatrix* m, int observer, int robot, int reading)
+void sensor_matrix_set_reading(int observer, int robot, int reading)
 {
-	if (robot >= m->size || observer >= m->size || m->matrix[observer] == NULL)
-		sensor_matrix_add_robot(m, observer);
-	m->matrix[observer][robot] = reading;
+#ifndef BAYBOARD
+	if (robot >= MAXIMUM_XBEE_ID || observer >= MAXIMUM_XBEE_ID)
+	{
+		WL_DEBUG_PRINT("ID too large.");
+		return;
+	}
+
+	m.matrix[observer][robot] = (unsigned char)reading;
+#endif
 }
 
 /**
  * Gets the sensor reading for a robot to another robot.
  *
- * @param m the sensor matrix
  * @param observer the robot whose reading we check
  * @param robot the robot who we are checking the reading to
  *
  * @return the observer's BOM reading for robot
  **/
-int sensor_matrix_get_reading(SensorMatrix* m, int observer, int robot)
+int sensor_matrix_get_reading(int observer, int robot)
 {
-	if (observer >= m->size || robot >= m->size)
+#ifndef BAYBOARD
+	if (observer >= MAXIMUM_XBEE_ID || robot >= MAXIMUM_XBEE_ID)
 		return -1;
-	return m->matrix[observer][robot];
+
+	return (int)m.matrix[observer][robot];
+#else
+	return -1;
+#endif
 }
 
 /**
  * Sets whether or not the given robot is part of the token ring.
  *
- * @param m the sensor matrix
  * @param robot the robot to set as a member / nonmember of the token ring
  * @param in 1 if the robot is in the token ring, 0 otherwise
  **/
-void sensor_matrix_set_in_ring(SensorMatrix* m, int robot, int in)
+void sensor_matrix_set_in_ring(int robot, int in)
 {
-	if (robot >= m->size)
-		sensor_matrix_expand(m, robot + 1);
-	if (in == 1)
-		sensor_matrix_add_robot(m, robot);
-	if (in == 1 && m->joined[robot] == 0)
-		m->numJoined++;
-	if (in == 0 && m->joined[robot] == 1)
-		m->numJoined--;
-	m->joined[robot] = in;
+	if (robot >= MAXIMUM_XBEE_ID)
+	{
+		WL_DEBUG_PRINT("ID too large.");
+		return;
+	}
+
+	if (in == 1 && m.joined[robot] == 0)
+		m.numJoined++;
+
+	if (in == 0 && m.joined[robot])
+		m.numJoined--;
+	m.joined[robot] = in;
 }
 
 /**
  * Checks if the given robot is in the token ring.
- * 
- * @param m the sensor matrix
+ *
  * @param robot the ID of the robot to check
  *
  * @return 1 if the robot is in the token ring, 0 otherwise
  **/
-int sensor_matrix_get_in_ring(SensorMatrix* m, int robot)
+int sensor_matrix_get_in_ring(int robot)
 {
-	if (robot >= m->size)
+	if (robot >= MAXIMUM_XBEE_ID)
 		return -1;
-	return m->joined[robot];
+
+	return m.joined[robot];
 }
 
 /**
- * Returns the size of the sensor matrix.
+ * Returns the number of robots which have joined the
+ * token ring.
  *
- * @param m the sensor matrix
- * 
- * @return the size of the sensor matrix
+ * @return the number of robots in the token ring
  **/
-int sensor_matrix_get_size(SensorMatrix* m)
+int sensor_matrix_get_joined(void)
 {
-	return m->size;
+	return m.numJoined;
 }
 
 /**
- * Returns the number of robots which have joined the
- * token ring.
+ * Returns the maximum XBee id of a robot which can be stored
+ * in the sensor matrix.
  *
- * @param m the sensor matrix
- *
- * @return the number of robots in the token ring
+ * @return the maximum number of robots which can be stored
  **/
-int sensor_matrix_get_joined(SensorMatrix* m)
+int sensor_matrix_get_size(void)
 {
-	return m->numJoined;
+	return MAXIMUM_XBEE_ID;
 }
 
Index: branches/encoders/code/projects/libwireless/lib/wireless.c
===================================================================
--- branches/encoders/code/projects/libwireless/lib/wireless.c	(revision 1344)
+++ branches/encoders/code/projects/libwireless/lib/wireless.c	(revision 1345)
@@ -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,38 +40,36 @@
 #include "wl_defs.h"
 
 #ifndef ROBOT
-#include <time.h>
-#include <signal.h>
+	#include <sys/time.h>
+	#include <signal.h>
 #else
-#include <time.h>
-#ifndef FIREFLY
-#include <bom.h>
+	#include <time.h>
+	#include <bom.h>
 #endif
-#endif
 
 /*Function Prototypes*/
 
-void wl_do_timeout(void);
+static void wl_do_timeout(void);
 
 //Note: the actual frame sent has group as the first four bits and
 //frame as the last four.
-void wl_send_packet(char group, char type, char* data, int len,
-					int dest, char options, char frame);
+static int wl_send_packet(char group, char type, char* data, int len, int dest, char options, char frame);
 
 /*Data Members*/
 
 //used to store incoming and outgoing packets
-unsigned char wl_buf[128];
+//TODO: does this need to be 128?  can it be smaller to save memory?
+//TODO: this shouldn't be hardcoded as 128.  it should be a define.
+static unsigned char wl_buf[128];
 //1 if we have timed out since we last checked, 0 otherwise.
-int wl_timeout = 0;
+static int wl_timeout = 0;
 
-PacketGroupHandler* wl_packet_groups[WL_MAX_PACKET_GROUPS];
+static 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)
+static void sig_handler(int signo)
 {
 	switch (signo)
 	{
@@ -51,51 +83,74 @@
 	}
 	return;
 }
+#else
+
+//called when the timer ticks
+static void timer_handler(void)
+{
+	wl_timeout = 1;
+}
+
 #endif
 
 /**
  * Initializes the wireless library. Must be called before any
  * other function.
+ *
+ * @param wl_port File descriptor for wireless port, or NULL for default.
  **/
-void wl_init()
+int wl_init()
 {
 	int i;
+  //TODO: using memset here instead of this loop, *might* be less instructions and *might* reduce code size but not sure
 	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);
+#ifdef ROBOT
+#ifdef FIREFLY
+	rtc_init(PRESCALE_DIV_256, 32, &timer_handler);
+#else
+	//TODO: FIX THIS
+	#ifdef BAYBOARD
+	rtc_init(10 * HALF_SECOND, &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); 
+#endif
+#else
+
+	//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
+#endif
+
+	return 0;
 }
 
 /**
@@ -103,16 +158,14 @@
  **/
 void wl_terminate()
 {
-	#ifndef ROBOT
-	timer_delete(wl_timeout_timer);
-	#endif
-	
 	int i;
-	for (i = 0; i < WL_MAX_PACKET_GROUPS; 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 != NULL) {
 			wl_packet_groups[i]->unregister();
-	
+		}
+	}
+
 	xbee_terminate();
 }
 
@@ -123,9 +176,11 @@
  *
  * @see wl_get_pan
  **/
-void wl_set_pan(int pan)
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
+int wl_set_pan(int pan)
 {
-	xbee_set_pan_id(pan);
+	return xbee_set_pan_id(pan);
 }
 
 /**
@@ -135,6 +190,8 @@
  *
  * @see wl_set_pan
  **/
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
 int wl_get_pan(void)
 {
 	return xbee_get_pan_id();
@@ -147,9 +204,11 @@
  *
  * @see wl_get_channel
  **/
-void wl_set_channel(int channel)
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
+int wl_set_channel(int channel)
 {
-	xbee_set_channel(channel);
+	return xbee_set_channel(channel);
 }
 
 /**
@@ -159,6 +218,8 @@
  *
  * @see wl_set_channel
  **/
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
 int wl_get_channel(void)
 {
 	return xbee_get_channel();
@@ -169,7 +230,9 @@
  *
  * @return the 16-bit address of the XBee module.
  **/
-unsigned int wl_get_xbee_id()
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
+int wl_get_xbee_id()
 {
 	return xbee_get_address();
 }
@@ -184,11 +247,11 @@
  * @param dest the 16-bit address of the XBee to send the packet to
  * @param frame the frame number to see with a TX_STATUS response
  **/
-void wl_send_robot_to_robot_global_packet(char group, char type,
-		char* data, int len, int dest, char frame)
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
+int wl_send_robot_to_robot_global_packet(char group, char type, char* data, int len, int dest, char frame)
 {
-	wl_send_packet(group, type, data, len, dest,
-			XBEE_OPTIONS_BROADCAST_ALL_PANS, frame);
+	return wl_send_packet(group, type, data, len, dest, XBEE_OPTIONS_BROADCAST_ALL_PANS, frame);
 }
 
 /**
@@ -201,11 +264,11 @@
  * @param dest the 16-bit address of the XBee to send the packet to
  * @param frame the frame number to see with a TX_STATUS response
  **/
-void wl_send_robot_to_robot_packet(char group, char type,
-		char* data, int len, int dest, char frame)
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
+int wl_send_robot_to_robot_packet(char group, char type, char* data, int len, int dest, char frame)
 {
-	wl_send_packet(group, type, data, len, dest, XBEE_OPTIONS_NONE,
-			frame);
+	return wl_send_packet(group, type, data, len, dest, XBEE_OPTIONS_NONE, frame);
 }
 
 /**
@@ -217,11 +280,11 @@
  * @param len the packet length in bytes
  * @param frame the frame number to see with a TX_STATUS response
  **/
-void wl_send_global_packet(char group, char type,
-		char* data, int len, char frame)
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
+int wl_send_global_packet(char group, char type, char* data, int len, char frame)
 {
-	wl_send_packet(group, type, data, len, XBEE_BROADCAST,
-			XBEE_OPTIONS_BROADCAST_ALL_PANS, frame);
+	return wl_send_packet(group, type, data, len, XBEE_BROADCAST, XBEE_OPTIONS_BROADCAST_ALL_PANS, frame);
 }
 
 /**
@@ -233,10 +296,11 @@
  * @param len the packet length in bytes
  * @param frame the frame number to see with a TX_STATUS response
  **/
-void wl_send_pan_packet(char group, char type,
-		char* data, int len, char frame)
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
+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);
 }
 
@@ -251,18 +315,21 @@
  * @param options the options for sending the packet
  * @param frame the frame number to see with a TX_STATUS response
  **/
-void wl_send_packet(char group, char type, char* data, int len,
-					int dest, char options, char frame)
+int wl_send_packet(char group, char type, char* data, int len, int dest, char options, char frame)
 {
+  //TODO: does this need to be 128?  can it be smaller to save memory?
+  //TODO: this shouldn't be hardcoded as 128.  it should be a define.
 	char buf[128];
 	int i;
 	if (frame != 0)
 		frame = (frame & 0x0F) | ((group & 0x0F) << 4);
+
 	buf[0] = group;
 	buf[1] = type;
 	for (i = 0; i < len; i++)
 		buf[2 + i] = data[i];
-	xbee_send_packet(buf, len + 2, dest, options, frame);
+
+	return xbee_send_packet(buf, len + 2, dest, options, frame);
 }
 
 /**
@@ -289,7 +356,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 +394,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,39 +406,42 @@
 			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);
 		int success = 0;
 		if (wl_buf[2] == 0)
+		{
 			success = 1;
+		}
 		else
 		{
 			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(
-					(int)wl_buf[1] & 0x0F, success);
-		return;
+
+		if (wl_packet_groups[group] != NULL && wl_packet_groups[group]->handle_response != NULL)
+			wl_packet_groups[group]->handle_response((int)wl_buf[1] & 0x0F, success);
 	}
-	
-	if (wl_buf[0] == XBEE_RX)
+	else if (wl_buf[0] == XBEE_RX)
 	{
+    //TODO: what does this 7 represent?  It shouldn't be hardcoded.  It should be set as a define
 		if (len < 7)
 		{
 			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 +448,31 @@
 		//1 for Address broadcast, 2 for PAN broadcast
 		int options = wl_buf[4];
 		*/
-		
+
+    //TODO: these indices, etc should be defined, not hardcoded
 		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_buf + 7, packetLen);
-		return;
+
+		if (wl_packet_groups[group] != NULL && wl_packet_groups[group]->handle_receive != NULL) {
+			wl_packet_groups[group]->handle_receive(type, source, wl_buf + 7, packetLen);
+		}
 	}
-	
-	WL_DEBUG_PRINT("Unexpected packet received from XBee.\r\n");
-	return;
+	else
+	{
+		WL_DEBUG_PRINT("Unexpected packet received from XBee.\r\n");
+		printf("0x%2X\n", wl_buf[0]);
+		printf("%c%c%d\n", wl_buf[2], wl_buf[3], wl_buf[4]);
+	}
 }
 
+
+#ifndef ROBOT
+//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
+// it reduces code size or not should be done to be sure.
+void wl_set_com_port(char* port)
+{
+	xbee_set_com_port(port);
+}
+#endif
+
Index: branches/encoders/code/projects/libdragonfly/lights.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/lights.h	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/lights.h	(revision 1345)
@@ -1,4 +1,34 @@
+// FIXME remove
+typedef unsigned char uint8_t;
+
+
 /**
+ * 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 lights.h
  * @brief Contains declarations for managing the orbs.
  *
@@ -11,13 +41,82 @@
 #ifndef _LIGHTS_H_
 #define _LIGHTS_H_
 
+/**
+ * @addtogroup orbs
+ * @{
+ **/
 
 /**
+ * Quick start: call orb_init_pwm or orb_init_binary, depending on which mode you want to use. Call orb*set or
+ * orb*set_color to set the orbs.
+ * 
+ * The orbs have two modes of operation: PWM mode and binary mode. In PWM mode, a pwm signal is generated by a hardware
+ * timer and the orbs can be set to a value of 0 through 255. In binary mode, the orbs can only be turned on or off and
+ * a value of 0 means "off" and any other value means "on". The mode can be chosen on initialization and can be changed
+ * at runtime using the orb_set_mode function.
+ *
+ * Operation (PWM mode): On timer overflow, all LEDs with a value>0 are turned on and the output compare value for the
+ * first LED is loaded. On compare match, the corresponding LED is turned off and the next output compare value is
+ * loaded. All masks are precomputed and sorted by time when setting the values.
+ *
+ * The data structure (pwm_t) containing the PWM times and masks is triple buffered. This is because the buffer the ISR
+ * is reading from may only be modified on timer overflow before the next PWM sequence is started, because otherwise the
+ * next OCR value might be sed to a value smaller than the current timer value, resulting in the remaining channels not
+ * being turned off in that PWM period (flash to on). When using two buffers, the page flip can only occur on a timer
+ * overflow for the same reason. So after writing a buffer and marking it for page flip, neither of the buffers could be
+ * modified because the front buffer is read by the ISR and the back buffer could be switched at any time. So the
+ * calling thread would have to be delayed by up to one full PWM period (8ms in the current implementation, but
+ * 20ms-50ms would be a reasonable value to expect here). To avoid this, triple buffering is used.
+ *
+ * The code for applying the orbs is fairly optimized. See the apply_orbs function for some time measurements and
+ * further nodes.
+ *
+ * The PWM frequency is 120Hz (8ms period time). The next lower frequency (determined by the prescaler) is 30 Hz which
+ * is too slow (orbs flicker).
+ *
+ * The orbs code is thread safe, which means that the functions may be called from another interrupt handler. If there
+ * are multiple concurrent calls to the orb*set* functions, one of them is ignored and the orbs are never left in an
+ * inconsistent state. For example, if the orbs are set to green by the main thread and to red by an interrupt handler,
+ * the resulting color will be either red or green, but never yellow.
+ * Thread safety is achieved by grabbing a lock at the beginning of all functions that modify the orb code and releasing
+ * the lock at the end. If the lock is already taken, the function just returns doing nothing.
+ *
+ * Some performance measurements:
+ *   - Time for setting new orb values (PWM mode):       35us-72us (depending on the degree to which the array is
+ *                                                                  already in the correct order)
+ *   - Time for setting new orb values (binary mode):        5.5us
+ *
+ *   - Interrupt time (PWM mode only):                         8us (overflow)
+ *                                                            10us (output compare)
+ *                                                             6us (last output compare)
+ *                                                            30us (output compare, all value equal)
+ *
+ *   - Maximum total interrupt time per period:               64us
+ *   - Maximum CPU usage for interrupts (PWM mode only):     <0.8%
+ *
+ *   - Maximum contiguous synchronized block:                 30us (output compare interrupt, all values equal)
+ *
+ * There are some potential optimizations left. See the source code for more information.
+ *
+ * A note on robustness: if the output compare interrupt is disabled for too long, either due to a long ISR or a long
+ * synchronized code block, the orbs will flicker to brighter values for being turned off too late. With software PWM,
+ * there's nothing at all to be done about that. The problem can be alleviated by using a lower PWM frequency, but then
+ * the orbs will start flickering all the time due to the low update frequency.
+ * Some measurements: with 100us synchronized blocks, the flickering is accepptably low. Longer synchronized blocks
+ * mean more flickering. At 1ms synchronized blocks, the flickering is quite bad, especially for low orb values. Note
+ * that orb value 0 never flickers at all because the corresponding channels are not turned on at all.
+ * Test code (note the _delay_us restrictions!)
+ *   orb_set (1,1,1); while (1) { SYNC { for (uint8_t m=0; m<10; ++m) { _delay_us(10); } } }
+ **/
+
+/** @} **/ //end addtogroup
+
+/**
  * @addtogroup orbs
  * @{
  **/
 
-//ORB Colors
+// ***** Predefined colors *****
 /** @brief Red **/
 #define RED       0xE0
 /** @brief Orange **/
@@ -40,34 +139,84 @@
 #define MAGENTA   0xE3
 /** @brief White **/
 #define WHITE     0xFE
-/** @brief Turn the orb off (White) **/
-#define ORB_OFF   0xFE      //ORB_OFF->WHITE
+/** @brief Turn the orb off **/
+#define ORB_OFF   0x00
 
-/** @brief Enables the orbs **/
+
+
+// ***** Initialization *****
+
+/** @brief Enables the orbs in default mode **/
 void orb_init(void);
+
+/** @brief Enables the orbs in binary mode **/
+void orb_init_binary (void);
+
+/** @brief Enables the orbs in PWM mode **/
+void orb_init_pwm (void);
+
+
+
+// ***** Mode setting *****
+
+/** Specification of the orb mode **/
+typedef uint8_t orb_mode_t;
+
+/** @brief PWM mode **/
+#define orb_mode_pwm 0
+
+/** @brief Binary mode **/
+#define orb_mode_binary 1
+
+
+/** @brief Switches the orbs to the specified mode **/
+void orb_set_mode (orb_mode_t mode);
+
+/** @brief Disables the orb timer, but does not change the mode **/
+void orb_disable_timer (void);
+
+/** @brief Enables the orb timer, but does not change the mode **/
+void orb_enable_timer (void);
+
+
+
+// ***** Setting RGB colors *****
+
+/** @brief set the specified orb to a specified color **/
+void orb_n_set (uint8_t num, uint8_t red, uint8_t green, uint8_t blue);
+
 /** @brief Set both orbs to a specified color **/
-void orb_set(unsigned char red_led, unsigned char green_led,
-	unsigned char blue_led);
+void orb_set(uint8_t red, uint8_t green, uint8_t blue);
+
 /** @brief Set orb1 to a specified color **/
-void orb1_set(unsigned char red_led, unsigned char green_led,
-	unsigned char blue_led); 
+void orb1_set(uint8_t red_led, uint8_t green_led, uint8_t blue_led); 
+
 /** @brief Set orb2 to a specified color **/
-void orb2_set(unsigned char red_led, unsigned char green_led,
-	unsigned char blue_led);
+void orb2_set(uint8_t red_led, uint8_t green_led, uint8_t blue_led);
 
-/** @brief Set both orbs to a specified color **/
-void orb_set_color(int col);
+void orbs_set (uint8_t red1, uint8_t green1, uint8_t blue1, uint8_t red2, uint8_t green2, uint8_t blue2);
+
+
+
+// ***** Settings predefined colors *****
+
+/** @brief set the specified orb to the specified color **/
+void orb_n_set_color(uint8_t num, uint8_t col);
+
 /** @brief Set orb1 to a specified color **/
-void orb1_set_color(int col);
+void orb1_set_color(uint8_t col);
+
 /** @brief Set orb2 to a specified color **/
-void orb2_set_color(int col);
+void orb2_set_color(uint8_t col);
 
-/** @brief Disable the orbs **/
-void orb_disable(void);
-/** @brief Enable the orbs **/
-void orb_enable(void);
+/** @brief set the orbs to specified colors **/
+void orbs_set_color(uint8_t col1, uint8_t col2);
 
+/** @brief Set both orbs to a specified color **/
+void orb_set_color(uint8_t col);
+
+
+
 /** @} **/ //end addtogroup
 
 #endif
-
Index: branches/encoders/code/projects/libdragonfly/motor.c
===================================================================
--- branches/encoders/code/projects/libdragonfly/motor.c	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/motor.c	(revision 1345)
@@ -1,14 +1,39 @@
-/*
-	motor.c - Contains functions necessary for activating and driving the 
-	H-bridge
-	
-	
-	author: Robotics Club, Colony project
-	
-	much of this is taken from FWR's library, author: Tom Lauwers
-	
-*/
+/**
+ * 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 motor.c
+ * @brief Motors
+ *
+ * Implementation of functions for controlling the motors.
+ *
+ * @author Colony Project, CMU Robotics Club
+ * Much of this is taken from FWR's library, author: Tom Lauwers
+ **/
+
 #include "motor.h"
 
 /**
@@ -25,21 +50,21 @@
  * @see motors_off, motor1_set, motor2_set
  **/
 void motors_init( void ) {
-	// Configure counter such that we use phase correct
-	// PWM with 8-bit resolution
-	PORTA &= 0x0F;
-	DDRA |= 0xF0;
-	DDRB |= 0x60;
+  // Configure counter such that we use phase correct
+  // PWM with 8-bit resolution
+  PORTA &= 0x0F;
+  DDRA |= 0xF0;
+  DDRB |= 0x60;
 
-	//timer 1A and 1B
-	TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);
-	TCCR1B = _BV(WGM12) | _BV(CS10);
-//	TCCR1A = 0xA1;
-//	TCCR1B = 0x04;
-	OCR1AH=0;
-	OCR1AL=0;
-	OCR1BH=0;
-	OCR1BL=0;
+  //timer 1A and 1B
+  TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);
+  TCCR1B = _BV(WGM12) | _BV(CS10);
+  //	TCCR1A = 0xA1;
+  //	TCCR1B = 0x04;
+  OCR1AH=0;
+  OCR1AL=0;
+  OCR1BH=0;
+  OCR1BL=0;
 }
 
 /**
@@ -53,29 +78,26 @@
  **/
 void motor1_set(int direction, int speed) {
 
-	if(direction == 0) {
-	    // turn off PWM first if switching directions
-		if((PORTA & 0x30) != 0x10)
-		{
-		  OCR1A = 0;
-		}	
-		PORTA = (PORTA & 0xCF) | 0x10;
-//		PORTD |= 0x10;
-//		PORTD &= 0xBF;
-	}
-	else {
-	    // turn off PWM first if switching directions
-		if((PORTA & 0x30) != 0x20)
-		{
-		  OCR1A = 0;
-		}
-		PORTA = (PORTA & 0xCF) | 0x20;
-//		PORTD |= 0x40;
-//		PORTD &= 0xEF;
-	}
+  if(direction == 0) {
+    // turn off PWM first if switching directions
+    if((PORTA & 0x30) != 0x10) {
+      OCR1A = 0;
+    }	
+    PORTA = (PORTA & 0xCF) | 0x10;
+    //		PORTD |= 0x10;
+    //		PORTD &= 0xBF;
+  } else {
+    // turn off PWM first if switching directions
+    if((PORTA & 0x30) != 0x20) {
+      OCR1A = 0;
+    }
+    PORTA = (PORTA & 0xCF) | 0x20;
+    //		PORTD |= 0x40;
+    //		PORTD &= 0xEF;
+  }
 	
-	// Set the timer to count up to speed, an 8-bit value
-	OCR1AL = speed;
+  // Set the timer to count up to speed, an 8-bit value
+  OCR1AL = speed;
 }
 
 /**
@@ -88,30 +110,27 @@
  * @see motor1_set, motors_init
  **/
 void motor2_set(int direction, int speed) {
-	if(direction == 0) {
-//		PORTD |= 0x20;
-//		PORTD &= 0x7F;
-	    // turn off PWM first if switching directions
-		if((PORTA & 0xC0) != 0x80)
-		{
-		  OCR1B = 0;
-		}		
+  if(direction == 0) {
+    //		PORTD |= 0x20;
+    //		PORTD &= 0x7F;
+    // turn off PWM first if switching directions
+    if((PORTA & 0xC0) != 0x80) {
+      OCR1B = 0;
+    }		
 		
-		PORTA = (PORTA & 0x3F) | 0x80;
-	}
-	else {
-//		PORTD |= 0x80;
-//		PORTD &= 0xDF;
+    PORTA = (PORTA & 0x3F) | 0x80;
+  } else {
+    //		PORTD |= 0x80;
+    //		PORTD &= 0xDF;
 
-	    // turn off PWM first if switching directions
-		if((PORTA & 0xC0) != 0x40)
-		{
-		  OCR1B = 0;
-		}		
+    // turn off PWM first if switching directions
+    if((PORTA & 0xC0) != 0x40) {
+      OCR1B = 0;
+    }		
 		
-		PORTA = (PORTA & 0x3F) | 0x40;
-	}
-	OCR1BL = speed;
+    PORTA = (PORTA & 0x3F) | 0x40;
+  }
+  OCR1BL = speed;
 }
 
 /**
@@ -120,8 +139,8 @@
  * @see motors_init
  **/
 void motors_off( void ) {
-	OCR1AL = 0x0;
-	OCR1BL = 0x0;
+  OCR1AL = 0x0;
+  OCR1BL = 0x0;
 }
 
 /**@}**///end defgroup
Index: branches/encoders/code/projects/libdragonfly/motor.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/motor.h	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/motor.h	(revision 1345)
@@ -1,4 +1,30 @@
 /**
+ * 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 motor.h
  * @brief Contains definitions for controlling the motors
  *
Index: branches/encoders/code/projects/libdragonfly/dragonfly_lib.c
===================================================================
--- branches/encoders/code/projects/libdragonfly/dragonfly_lib.c	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/dragonfly_lib.c	(revision 1345)
@@ -1,3 +1,38 @@
+/**
+ * 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 dragonfly_lib.c
+ * @brief Dragonfly initialization
+ *
+ * Contains implementation of dragonfly_init.
+ *
+ * @author Colony Project, CMU Robotics Club
+ **/
+
 #include <dragonfly_lib.h>
 
 /* init_dragonfly - Initializes functions based on configuration parameters
@@ -27,57 +62,79 @@
  * @see analog_init, usb_init, xbee_init, buzzer_init,
  * bom_init, orb_init, motors_init, lcd_init
  **/
+ 
+void flash_red(void);
+
 void dragonfly_init(int config) 
 {
-	// Set directionality of various IO pins
-	DDRG &= ~(_BV(PING0)|_BV(PING1));
-	PORTG |= _BV(PING0)|_BV(PING1);
-	
-	if(config & ANALOG) {
-		analog_init();
-	}
-	
-	if(config & COMM) {
-    //Defaults to 115200. Check serial.h for more information.
     sei();
-    usb_init();
-    xbee_init();
-	}
-	
-	if(config & BUZZER) {
-    sei();
-		buzzer_init();
-	}
-	
-	if(config & ORB) {
-    sei();
-		orb_init();
-	}
-	
-	if(config & MOTORS) {
-		motors_init();
-	}
-	
-	if(config & SERVOS) {
-		sei();
-    //Servos not yet implemented
-    //servo_init();
-	}
+    // Set directionality of various IO pins
+    DDRG &= ~(_BV(PING0)|_BV(PING1));
+    PORTG |= _BV(PING0)|_BV(PING1);
+    
+    if(config & ANALOG)
+        analog_init(ADC_START);
+    
+    if(config & COMM)
+    {
+        //Defaults to 115200. Check serial.h for more information.
+        usb_init();
+        xbee_init();
+    }
+    
+    if(config & BUZZER)
+    {
+        sei();
+        buzzer_init();
+    }
+    
+    if(config & ORB)
+    {
+        sei();
+        orb_init();
+    }
+    
+    if(config & MOTORS)
+        motors_init();
 
-	if(config & LCD) {
-		lcd_init();
-	}
-	
-	if(config & (SERVOS | ORB)) {
+    if(config & LCD)
+        lcd_init();
+    
+    if(config & RANGE)
+        range_init();
+        
+    if(config & BOM)
+    {
+        unsigned char bom_read = get_bom_type();
+        if(bom_read == 0xFF)
+            //warn that bom initialization failed
+            flash_red();
+        else
+            bom_init(bom_read);
+    }
+    // delay a bit for stability
+    _delay_ms(1);
+}
+
+//flash lights red three times and restore ports
+void flash_red(void)
+{
+    cli();
+    char dd = DDRC;
+    char po = PORTC;
+    char i;
+    DDRC = 0x77;
+    for(i = 0; i<3; i++)
+    {
+        PORTC = 0x77;
+        delay_ms(300);
+        PORTC = 0x66;
+        delay_ms(300);
+    }
+    
+    DDRC = dd;
+    PORTC = po;
     sei();
-    orb_init();
-    //Servos not yet implemented
-	}
-	
-	// delay a bit for stability
-	_delay_ms(1);
 }
-
-
 /** @} **/ //end defgroup
 
Index: branches/encoders/code/projects/libdragonfly/i2c.c
===================================================================
--- branches/encoders/code/projects/libdragonfly/i2c.c	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/i2c.c	(revision 1345)
@@ -1,5 +1,33 @@
-/* @file i2c.c
- * @brief
+/**
+ * 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 i2c.c
+ * @brief Implemenation of I2C communications protocol
+ * 
  * In the case where you have master sends and then a master request to the same
  * address, you will not give up control of the line because the send and
  * request addresses are seen as different addresses. In between it will send a
@@ -73,6 +101,8 @@
  *								from the master
  * @param slave_send		The address of the function to call when you are a slave and the master
  *								requests data from you.
+ *
+ * @return 0 for success, nonzero for failure
  **/
 int i2c_init(char addr, fun_mrecv_t master_recv, fun_srecv_t slave_recv, fun_send_t slave_send) {
     master_recv_function = master_recv;
@@ -105,6 +135,8 @@
  * @param data 	The pointer to the byte array of data
  * @param bytes	The amount of bytes long that the byte array is. This is how
  *						many bytes from the array that the function will send.
+ *
+ * @return zero for success, nonzero for failure
  **/
 int i2c_send(char dest, char *data, size_t bytes) {
     int i;
@@ -144,6 +176,8 @@
  * to send it.
  *
  * @param dest		The destination that we want to receive information from.
+ *
+ * @return 0 for success, nonzero for failure
  **/ 
 int i2c_request(char dest) {
     if(RING_BUFFER_FULL(i2c_write_buff))
@@ -160,6 +194,8 @@
   
     return 0;
 }
+
+/** @} **/
  
 /**
  * @brief Interrupt to handle I2C interrupts from the I2C hardware.
@@ -175,7 +211,6 @@
  *
  * Errors are handled here as well.
  **/ 
- /* @} */
 ISR(TWI_vect) {
 	static char data_to_send;
 	static char addr_to_send = -1;
@@ -307,3 +342,4 @@
   /* Toggle TWINT so that it resets and executes the commands */
   TWCR |= _BV(TWINT);
 }
+
Index: branches/encoders/code/projects/libdragonfly/serial.c
===================================================================
--- branches/encoders/code/projects/libdragonfly/serial.c	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/serial.c	(revision 1345)
@@ -1,10 +1,38 @@
-/*
-	serial.c - Functions for using the RS232 serial port
-	
-	authors: Robotics Club, Colony Project, pkv
-	much code taken from FWR's library, author: Tom Lauwers
-*/
+/**
+ * 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 serial.c
+ * @brief Serial Input and Output
+ *
+ * Implementation of functions for serial input and output.
+ *
+ * @author Colony Project, CMU Robotics Club
+ **/
+
 #include <avr/io.h>
 #include <stdio.h>
 #include "serial.h"
@@ -21,29 +49,29 @@
   //Set baud rate
   // - 115200 (both wired and wireless) is UBRR=8, U2X=1
   // - 9600 is U2X =1, UBRR = 107.
-	#if (USB_BAUD == 115200)
-    UBRR0H = 0x00;
-    UBRR0L = 8;
-    UCSR0A |= _BV(U2X0);
-  #elif (USB_BAUD == 9600)
-    UBRR0H = 0x00;
-    UBRR0L = 103;
-    UCSR0A |= _BV(U2X0);
-  #else //Baud rate is defined in the header file, we should not get here
-    return;
-  #endif
+#if (USB_BAUD == 115200)
+  UBRR0H = 0x00;
+  UBRR0L = 8;
+  UCSR0A |= _BV(U2X0);
+#elif (USB_BAUD == 9600)
+  UBRR0H = 0x00;
+  UBRR0L = 103;
+  UCSR0A |= _BV(U2X0);
+#else //Baud rate is defined in the header file, we should not get here
+  return;
+#endif
 
   /*Enable receiver and transmitter */
-	UCSR0B |= (1<<RXEN0)|(1<<TXEN0);
+  UCSR0B |= (1<<RXEN0)|(1<<TXEN0);
 	
-	/* Set frame format: 8data, 1stop bit, asynchronous normal mode */
-	UCSR0C |= (1<<UCSZ00) | (1<<UCSZ01);
+  /* Set frame format: 8data, 1stop bit, asynchronous normal mode */
+  UCSR0C |= (1<<UCSZ00) | (1<<UCSZ01);
   
   // if we have enabled the stdio stuff, then we init it here
-  #ifdef USE_STDIO
-    /* Open the stdio stream corresponding to this port */
-    usb_fd = fdevopen(usb_putc, usb_getc);
-  #endif
+#ifdef USE_STDIO
+  /* Open the stdio stream corresponding to this port */
+  usb_fd = fdevopen(usb_putc, usb_getc);
+#endif
 }
 
 /**
@@ -55,29 +83,29 @@
   //Set baud rate
   // - 115200 (both wired and wireless) is UBRR=8, U2X=1
   // - 9600 is U2X =1, UBRR = 107.
-	#if (XBEE_BAUD == 115200)
-    UBRR1H = 0x00;
-    UBRR1L = 8;
-    UCSR1A |= _BV(U2X1);
-  #elif (XBEE_BAUD == 9600)
-    UBRR1H = 0x00;
-    UBRR1L = 103;
-    UCSR1A |= _BV(U2X1);
-  #else //Baud rate is defined in the header file, we should not get here
-    return;
-  #endif
+#if (XBEE_BAUD == 115200)
+  UBRR1H = 0x00;
+  UBRR1L = 8;
+  UCSR1A |= _BV(U2X1);
+#elif (XBEE_BAUD == 9600)
+  UBRR1H = 0x00;
+  UBRR1L = 103;
+  UCSR1A |= _BV(U2X1);
+#else //Baud rate is defined in the header file, we should not get here
+  return;
+#endif
 
-	//Enable receiver and transmitter
-	UCSR1B |= (1<<RXEN1)|(1<<TXEN1);
+  //Enable receiver and transmitter
+  UCSR1B |= (1<<RXEN1)|(1<<TXEN1);
 	
-	// Set frame format: 8data, 1stop bit, asynchronous normal mode
-	UCSR1C |= (1<<UCSZ10) | (1<<UCSZ11);
+  // Set frame format: 8data, 1stop bit, asynchronous normal mode
+  UCSR1C |= (1<<UCSZ10) | (1<<UCSZ11);
   
   // if we have enabled the stdio stuff, then we init it here
-  #ifdef USE_STDIO
-    /* Open the stdio stream corresponding to this port */
-    xbee_fd = fdevopen(xbee_putc, xbee_getc);
-  #endif
+#ifdef USE_STDIO
+  /* Open the stdio stream corresponding to this port */
+  xbee_fd = fdevopen(xbee_putc, xbee_getc);
+#endif
 }
 
 /**
@@ -86,8 +114,7 @@
  * @param c the character to send
  * @return 0 for success, nonzero for failure
  **/
-int usb_putc(char c) 
-{
+int usb_putc(char c) {
   // Wait until buffer is clear for sending
   loop_until_bit_is_set(UCSR0A, UDRE0);
 	
@@ -102,8 +129,7 @@
  * @param c the character to send
  * @return 0 for success, nonzero for failure
  **/
-int xbee_putc(char c) 
-{
+int xbee_putc(char c) {
   // Wait until buffer is clear for sending
   loop_until_bit_is_set(UCSR1A, UDRE1);
 	
@@ -118,18 +144,32 @@
  * @param s the string to send
  * @return 0 for success, nonzero for failure
  **/
-int usb_puts(char *s)
-{
-	char *t = s;
-	while (*t != 0)
-	{
-		usb_putc(*t);
-		t++;
-	}
+int usb_puts(char *s) {
+  char *t = s;
+  while (*t != 0) {
+    usb_putc(*t);
+    t++;
+  }
   return 0;
 }
 
 /**
+ * Sends a sequence of characters from program space over USB.
+ *
+ * @param s the string to send
+ **/
+void usb_puts_P (PGM_P s) {
+    char buf;
+	
+    while (memcpy_P (&buf, s, sizeof (char)), buf!=0) {
+        usb_putc (buf);
+        s++;
+    }
+}
+
+
+
+/**
  * Returns the first character in the buffer received from USB.
  * This function blocks execution until a character has been received.
  * xbee_init must be called before this function may be used.
@@ -138,13 +178,12 @@
  *
  * @see usb_init, usb_getc_nb
  **/
-int usb_getc(void)
-{
+int usb_getc(void) {
   // Wait for the receive buffer to be filled
   loop_until_bit_is_set(UCSR0A, RXC0);
 	
-	// Read the receive buffer
-	return UDR0;
+  // Read the receive buffer
+  return UDR0;
 }
 
 /**
@@ -157,13 +196,12 @@
  * 
  * @see xbee_init, xbee_getc_nb
  **/
-int xbee_getc(void)
-{
-	// Wait for the receive buffer to be filled
-    loop_until_bit_is_set(UCSR1A, RXC1);
+int xbee_getc(void) {
+  // Wait for the receive buffer to be filled
+  loop_until_bit_is_set(UCSR1A, RXC1);
 	
-	// Read the receive buffer
-	return UDR1;
+  // Read the receive buffer
+  return UDR1;
 }
 
 /**
@@ -178,18 +216,16 @@
  * 
  * @see usb_init, usb_getc
  **/
-int usb_getc_nb(char *c)
-{
-	// check if the receive buffer is filled
+int usb_getc_nb(char *c) {
+  // check if the receive buffer is filled
   if (UCSR0A & _BV(RXC0)) {
     // Read the receive buffer
     (*c) = UDR0;
     return 0;
-  }
-  else {
+  } else {
     // Return empty
     return -1;
-	}
+  }
 }
 
 /**
@@ -204,26 +240,24 @@
  *
  * @see xbee_init, xbee_getc
  **/
-int xbee_getc_nb(char *c)
-{
-	// check if the receive buffer is filled
+int xbee_getc_nb(char *c) {
+  // check if the receive buffer is filled
   if (UCSR1A & _BV(RXC1)) {
     // Read the receive buffer
     (*c) = UDR1;
     return 0;
-  }
-  else {
+  } else {
     // Return empty
     return -1;
-	}
+  }
 }
 
 
 /*
-prints an int to serial
+  prints an int to serial
 
-code adapted from Chris Efstathiou's code (hendrix@otenet.gr)
-uses usb_putc
+  code adapted from Chris Efstathiou's code (hendrix@otenet.gr)
+  uses usb_putc
 */
 /**
  * Prints an integer, converted to ASCII, to usb. usb_init must be called
@@ -236,21 +270,74 @@
  * @see usb_init, usb_putc
  **/
 int usb_puti(int value ) {
-	unsigned char usb_data[6]={'0','0','0','0','0','0' }, position=sizeof(usb_data), radix=10; 
+  unsigned char usb_data[6]={'0','0','0','0','0','0' }, position=sizeof(usb_data), radix=10; 
 
-        /* convert int to ascii  */ 
-        if(value<0) { usb_putc('-'); value=-value; }    
-        do { position--; *(usb_data+position)=(value%radix)+'0'; value/=radix;  } while(value); 
+  /* convert int to ascii  */ 
+  if(value<0) { 
+    usb_putc('-'); 
+    value=-value; 
+  }    
+  do { 
+    position--; 
+    *(usb_data+position)=(value%radix)+'0'; 
+    value/=radix;  
+  } while(value); 
 
     
-        /* start displaying the number */
-        for(;position<=(sizeof(usb_data)-1);position++)
-          {
+  /* start displaying the number */
+  for(;position<=(sizeof(usb_data)-1);position++) {
             
-            usb_putc(usb_data[position]);
-          }
+    usb_putc(usb_data[position]);
+  }
 
-	return 0;
+  return 0;
 }
 
+/**
+ * Determines a hexadecimal digit in ASCII code.
+ *
+ * @param value the value of the digit (0<=value<=15)
+ * 
+ * @return the hexadecimal digit in ASCII code, or '?'
+ * if the input is invalid.
+ **/
+uint8_t hex_digit (uint8_t value)
+{
+    if (value>15) return '?';
+    // Postcondition: 0<=x<=15
 
+    return "0123456789ABCDEF"[value];
+}
+
+/**
+ * Prints a fixed width hexadecimal representation of an unsigned
+ * 16 bit integer in ASCII code to USB.
+ * usb_init must be called before this function can be used.
+ *
+ * @param value the value to print
+ * 
+ * @see usb_init, usb_puti, usb_puts, usb_puth8, hex_digit
+ **/
+void usb_puth16 (uint16_t value)
+{
+    usb_putc (hex_digit((value >>12)&0xF));
+    usb_putc (hex_digit((value >>8 )&0xF));
+    usb_putc (hex_digit((value >>4 )&0xF));
+    usb_putc (hex_digit( value      &0xF));
+}
+
+/**
+ * Prints a fixed width hexadecimal representation of an unsigned
+ * 8 bit integer in ASCII code to USB.
+ * usb_init must be called before this function can be used.
+ *
+ * @param value the value to print
+ * 
+ * @see usb_init, usb_puti, usb_puts, usb_puth16, hex_digit
+ **/
+void usb_puth8(uint8_t value)
+{
+    usb_putc (hex_digit ((value)>>4 &0xF));
+    usb_putc (hex_digit ( value     &0xF));
+}
+
Index: branches/encoders/code/projects/libdragonfly/dragonfly_lib.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/dragonfly_lib.h	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/dragonfly_lib.h	(revision 1345)
@@ -1,74 +1,115 @@
-/**
- * @file dragonfly_lib.h
- * @brief Contains other include files
- * 
- * Include this file for all the functionality of libdragonfly.
- *
- * @author Colony Project, CMU Robotics Club
- **/
-
-#ifndef _DRAGONFLY_LIB_H_
-#define _DRAGONFLY_LIB_H_
-
-/**
- * @addtogroup dragonfly
- * @{
- **/
-
-// Configuration definitions
-/** @brief Initialize analog **/
-#define ANALOG 0x01
-/** @brief Initialize serial communications **/
-#define SERIAL 0x02
-/** @brief Initialize USB communications **/
-#define USB    0x02
-/** @brief Initialize communications **/
-#define COMM   0x02
-/** @brief Initialize the orb **/
-#define ORB    0x04
-/** @brief Initialize the motors **/
-#define MOTORS 0x08
-/** @brief Initialize the servos **/
-#define SERVOS 0x10
-/** @brief Initialize I2C **/
-#define I2C    0x20
-/** @brief Initialize the buzzer **/
-#define BUZZER 0x40
-/** @brief Initialize the LCD screen **/
-#define LCD    0x80
-/** @brief Initialize everything **/
-#define ALL_ON 0xFF
-
-
-/** @brief Initialize the board **/
-void dragonfly_init(int config);
-
-/** @} **/ //end addtogroup
-
-#include <inttypes.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <avr/io.h>
-#include <avr/interrupt.h>
-#include <util/delay.h>
-#include <util/twi.h>
-
-#include <analog.h>
-#include <dio.h>
-#include <time.h>
-#include <lcd.h>
-#include <lights.h>
-#include <motor.h>
-#include <serial.h>
-#include <buzzer.h>
-#include <rangefinder.h>
-#include <bom.h>
-#include <move.h>
-#include <reset.h>
-#include <math.h>
-
-#include <spi.h>
-#include <encoders.h>
-
-#endif
-
+/**
+ * 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 dragonfly_lib.h
+ * @brief Contains other include files
+ * 
+ * Include this file for all the functionality of libdragonfly.
+ *
+ * @author Colony Project, CMU Robotics Club
+ **/
+
+#ifndef _DRAGONFLY_LIB_H_
+#define _DRAGONFLY_LIB_H_
+
+/**
+ * @addtogroup dragonfly
+ * @{
+ **/
+
+// Configuration definitions
+/** @brief Initialize analog **/
+#define ANALOG 0x01
+/** @brief Initialize serial communications **/
+#define SERIAL 0x02
+/** @brief Initialize USB communications **/
+#define USB    0x02
+/** @brief Initialize communications **/
+#define COMM   0x02
+/** @brief Initialize the orb **/
+#define ORB    0x04
+/** @brief Initialize the motors **/
+#define MOTORS 0x08
+/** @brief Initialize I2C **/
+#define I2C    0x20
+/** @brief Initialize the buzzer **/
+#define BUZZER 0x40
+/** @brief Initialize the LCD screen **/
+#define LCD    0x80
+/** @brief Initialize the rangefinders **/
+#define RANGE  0x0100
+/** @brief Initialize the BOM **/
+#define BOM  0x0200
+/** @brief Initialize everything  **/
+#define ALL_ON 0x03FF
+
+/** @brief Initialize the board **/
+void dragonfly_init(int config);
+
+/** @} **/ //end addtogroup
+
+#include <inttypes.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <util/delay.h>
+#include <util/twi.h>
+
+// This file is included from the libdragonfly directory because it seems to be
+// missing from the AVR libc distribution.
+#include "atomic.h"
+
+#include <analog.h>
+#include <dio.h>
+#include <time.h>
+#include <lcd.h>
+#include <lights.h>
+#include <motor.h>
+#include <serial.h>
+#include <buzzer.h>
+#include <rangefinder.h>
+#include <bom.h>
+#include <move.h>
+#include <reset.h>
+#include <math.h>
+#include <eeprom.h>
+#include <stdbool.h>
+
+/** @brief shortcut for ATOMIC_BLOCK(ATOMIC_RESTORESTATE) **/
+#define SYNC ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
+
+/** @brief atomically grab a lock if it is free, return otherwise **/
+#define REQUIRE_LOCK_OR_RETURN(LOCK) do { SYNC { if (LOCK) return; LOCK=1; } } while (0)
+
+/** @brief atomically release a lock **/
+#define RELEASE_LOCK(LOCK) do { LOCK=0; } while (0)
+
+
+
+#endif
+
Index: branches/encoders/code/projects/libdragonfly/buzzer.c
===================================================================
--- branches/encoders/code/projects/libdragonfly/buzzer.c	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/buzzer.c	(revision 1345)
@@ -1,11 +1,40 @@
-/*
-  buzzer.c - Contains the functions necessary for running the buzzer
-  
-  author: Robotics Club, Colony Project
-  
-  much taken from FWR's firefly library (author: Tom Lauwers)
-*/
+/**
+ * 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 buzzer
+ * @brief Functions for using the buzzer
+ *
+ * Contains functions for using the buzzer.
+ *
+ * @author Colony Project, CMU Robotics Club
+ *
+ * much taken from FWR's firefly library (author: Tom Lauwers)
+ **/
+
 #include <avr/io.h>
 #include <buzzer.h>
 #include <time.h>
Index: branches/encoders/code/projects/libdragonfly/i2c.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/i2c.h	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/i2c.h	(revision 1345)
@@ -1,8 +1,36 @@
+/**
+ * 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 i2c.h
  *  @brief Header file for I2C
  *
- *  @author CMU Robotics Club, Kevin Woo, Suresh Nidhiry
- */
+ *  Contains functions for I2C.
+ *
+ *  @author Kevin Woo and Suresh Nidhiry, Colony Project, CMU Robotics Club
+ **/
 
 
 #ifndef _I2C_H_
@@ -10,13 +38,13 @@
 
 #include <stddef.h>
 
-/** @brief Address of slave receive handler function */
+/** @brief Address of slave receive handler function **/
 typedef void (*fun_srecv_t)(char);
 
-/** @brief Address of master receive handler function*/
+/** @brief Address of master receive handler function**/
 typedef int (*fun_mrecv_t)(char);
 
-/** @brief Address of slave send handler function*/
+/** @brief Address of slave send handler function**/
 typedef char (*fun_send_t)(void);
 
 int i2c_init(char addr, fun_mrecv_t master_recv, fun_srecv_t slave_recv, fun_send_t slave_send);
@@ -26,3 +54,4 @@
 void i2c_packet_rec (char i2c_byte);
 void i2c_packet_sniff(char data);
 #endif
+
Index: branches/encoders/code/projects/libdragonfly/serial.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/serial.h	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/serial.h	(revision 1345)
@@ -1,4 +1,30 @@
 /**
+ * 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 serial.h
  * @brief Contains declarations for serial input and output
  *
@@ -10,7 +36,7 @@
  **/
 
 /*
-	serial.h - Contains definitions and function prototypes for the RS232 serial port
+  serial.h - Contains definitions and function prototypes for the RS232 serial port
   author(s): pkv
   
   Directions:
@@ -19,13 +45,13 @@
   write characters to the serial ports.
   
   UART Mapping:
-    usb_*() -> UART0
-    xbee_*() -> UART1
+  usb_*() -> UART0
+  xbee_*() -> UART1
   
   Options: (Add the following defines to your code to configure this library)
-    #define USB_BAUD { 115200 | 9600 } <= pick ONE value from in here
-    #define XBEE_BAUD { 115200 | 9600 } <= pick ONE value from in here
-    #define USE_STDIO
+  #define USB_BAUD { 115200 | 9600 } <= pick ONE value from in here
+  #define XBEE_BAUD { 115200 | 9600 } <= pick ONE value from in here
+  #define USE_STDIO
   
   Note: If you enable USE_STDIO, the first init function that is called will 
   automatically be linked to stdin, stdout, and stderr.  To use the baud rate 
@@ -42,6 +68,9 @@
 #ifndef _SERIAL_H
 #define _SERIAL_H
 
+#include <inttypes.h>
+#include <avr/pgmspace.h>
+
 /**
  * @defgroup usb USB Input / Output
  * @brief Functions for USB input / output
@@ -67,9 +96,20 @@
 int usb_getc_nb(char *c);
 /** @brief Print a string to USB **/
 int usb_puts(char *s);
+/** @brief Print a string from program space to USB **/
+void usb_puts_P (PGM_P s);
 /** @brief Print an integer to USB **/
 int usb_puti(int value);
+/** @brief Determine a hexadecimal digit **/
+uint8_t hex_digit (uint8_t value);
+/** @brief Print a fixed width hexadecimal representation to USB **/
+void usb_puth16 (uint16_t value);
+/** @brief Print a fixed width hexadecimal representation to USB **/
+void usb_puth8(uint8_t value);
+/** @brief Alias for usb_puth16 **/
+static inline void usb_puth (uint16_t value) { usb_puth16 (value); };
 
+
 /** @} **/ //end addtogroup
 
 /**
@@ -98,6 +138,8 @@
 /** @brief Read a character from the XBee without blocking **/
 int xbee_getc_nb(char *c);
 
+
+
 /** @} **/ //end addtogroup
 
 #endif
Index: branches/encoders/code/projects/libdragonfly/buzzer.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/buzzer.h	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/buzzer.h	(revision 1345)
@@ -1,4 +1,30 @@
 /**
+ * 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 buzzer.h
  * @brief Contains definitions for using the buzzer.
  *
Index: branches/encoders/code/projects/libdragonfly/eeprom.c
===================================================================
--- branches/encoders/code/projects/libdragonfly/eeprom.c	(nonexistent)
+++ branches/encoders/code/projects/libdragonfly/eeprom.c	(revision 1345)
@@ -0,0 +1,55 @@
+#include <avr/io.h>
+#include "eeprom.h"
+
+int eeprom_put_byte(unsigned int uiAddress, unsigned char ucData) {
+    /* Wait for completion of previous write */
+    while(EECR & (1<<EEWE));
+    /* Set up address and data registers */
+    EEAR = uiAddress;
+    EEDR = ucData;
+    /* Write logical one to EEMWE */
+    EECR |= (1<<EEMWE);
+    /* Start eeprom write by setting EEWE */
+    EECR |= (1<<EEWE);
+    
+    return 0;
+}
+
+int eeprom_get_byte(unsigned int uiAddress, unsigned char *byte) {
+    /* Wait for completion of previous write */
+    while(EECR & (1<<EEWE));
+    /* Set up address register */
+    EEAR = uiAddress;
+    /* Start eeprom read by writing EERE */
+    EECR |= (1<<EERE);
+    /* get data from data register */
+    *byte=EEDR;
+    
+    return 0;
+}
+
+
+unsigned char get_robotid(void) {
+    unsigned char c0, c1, c2;
+    
+    eeprom_get_byte(EEPROM_ROBOT_ID_ADDR, &c0);
+    eeprom_get_byte(EEPROM_ROBOT_ID_ADDR+1, &c1);
+    eeprom_get_byte(EEPROM_ROBOT_ID_ADDR+2, &c2);
+    if(c0 == 'I' && c1 == 'D')
+        return c2;
+    else
+        return 0xFF;
+}
+
+unsigned char get_bom_type(void) {
+    unsigned char c0, c1, c2, c3;
+    
+    eeprom_get_byte(EEPROM_BOM_TYPE_ADDR, &c0);
+    eeprom_get_byte(EEPROM_BOM_TYPE_ADDR+1, &c1);
+    eeprom_get_byte(EEPROM_BOM_TYPE_ADDR+2, &c2);
+    eeprom_get_byte(EEPROM_BOM_TYPE_ADDR+3, &c3);
+    if(c0 == 'B' && c1 == 'O' && c2 == 'M')
+        return c3;
+    else
+        return 0xFF;
+}
Index: branches/encoders/code/projects/libdragonfly/spi.c
===================================================================
--- branches/encoders/code/projects/libdragonfly/spi.c	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/spi.c	(revision 1345)
@@ -2,7 +2,6 @@
  * @file spi.c
  * @brief Basic SPI module to handle encoders
  * @author Colony Project, CMU Robotics Club
- * @bug Not tested
  *	Need to move spi.h include into dragonfly_lib.h when stable
  **/
 
@@ -15,6 +14,12 @@
 static spi_fun_recv_t spi_recv_func; /* byte handler */
 static spi_fun_recv_complete_t spi_recv_complete_func; /*transmission completion handler */
 
+/** 
+* @brief Initialize SPI hardware for communication.
+* 
+* @param recv_func The function to be called for each byte of data received.
+* @param recv_complete_func  The function to be called at the end of a complete transmission.
+*/
 void spi_init (spi_fun_recv_t recv_func, spi_fun_recv_complete_t recv_complete_func)
 {
     /*  Enable Interrupt, Enable SPI Module, MSB First, Master Mode, Clock div = 64 */
@@ -35,7 +40,11 @@
     //usb_puts("\tspi.c Debug: SPI INITIALIZED\n");
 }
 
-/* Transfer a given byte to slave and receive a byte */
+/** 
+* @brief Transfer a given byte to slave and receive a byte 
+* 
+* @param bytes The number of bytes to be transferred.
+**/
 void spi_transfer(char bytes)
 {
     spi_bytes = bytes;
Index: branches/encoders/code/projects/libdragonfly/eeprom.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/eeprom.h	(nonexistent)
+++ branches/encoders/code/projects/libdragonfly/eeprom.h	(revision 1345)
@@ -0,0 +1,49 @@
+/**
+ * @file eeprom.h
+ * @brief handles eeprom storage for persistent data
+ *
+ * Contains functions and definitions for reading and writing to eeprom
+ *
+ * @author Colony Project, Brad Neuman
+ */
+ 
+ #ifndef _EEPROM_H_
+ #define _EEPROM_H_
+ 
+ #include <bom.h>
+ 
+ #define EEPROM_ROBOT_ID_ADDR 0x10
+ #define EEPROM_BOM_TYPE_ADDR 0x14
+ 
+ /** @brief store a byte to eeproem
+  *  @return 0 if success, nonzero on failure
+  */
+ int eeprom_put_byte(unsigned int addr, unsigned char byte);
+ 
+ /** @brief reads a byte from eeprom
+  *
+  *  Pass it thge address and a pointer to a byte where the byte at the
+  *  address will be stored
+  *
+  *  @return 0 if successful (byte is set to the eeprom value at addr), 
+  *  nonzero if there was a problem
+   */
+ int eeprom_get_byte(unsigned int addr, unsigned char *byte);
+ 
+ /** @brief get stored robot ID
+  *
+  *  checks that EEPROM has been programed with an ID and returns it
+  *
+  *  @return the robot id, if it is stored. If it returns 0xFF it is probably invalid
+  */
+ unsigned char get_robotid(void);
+  
+   /** @brief get stored robot ID
+  *
+  * checks that EEPROM has been programed with an BOM type and returns it
+  *
+  *  @return the robot bom type as defined in bom.h, if it is stored. If it returns 0xFF it is probably invalid
+  */
+ unsigned char get_bom_type(void);
+ 
+ #endif
Index: branches/encoders/code/projects/libdragonfly/Makefile
===================================================================
--- branches/encoders/code/projects/libdragonfly/Makefile	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/Makefile	(revision 1345)
@@ -69,7 +69,6 @@
 CDEFS += -DF_CPU=$(F_CPU)UL 
 
 # Place -I options here
-CINCS =
 
 
 #---------------- Compiler Options ----------------
@@ -162,9 +161,9 @@
 
 publish:
 	$(COPY) $(TARGET).a $(COLONYROOT)/code/lib/bin
-	$(COPY) *.c $(COLONYROOT)/code/lib/src/$(TARGET)
 	$(COPY) *.h $(COLONYROOT)/code/lib/include/$(TARGET)
 
+.PHONY: docs
 docs:
 	$(DOXYGEN)
 	$(COPY) docs/html/* $(COLONYROOT)/docs/$(TARGET)
Index: branches/encoders/code/projects/libdragonfly/time.c
===================================================================
--- branches/encoders/code/projects/libdragonfly/time.c	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/time.c	(revision 1345)
@@ -1,28 +1,59 @@
+/**
+ * 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 time.c
+ * @brief Timer code
+ *
+ * Implementation of functions for timers.
+ *
+ * @author Colony Project, CMU Robotics Club
+ **/
+
 /*
-time.c
-anything that requires a delay
-mostly delay_ms
+  time.c
+  anything that requires a delay
+  mostly delay_ms
 
-author: Robotics Club, Colony Project
+  author: Robotics Club, Colony Project
 
-Change Log:
-	2.5.07 - Kevin
-		Aaron fixed the orb/servo code and made them use timer3 but compare registers B and C. He hard set the prescaler
-		to 8 so the RTC broke. Changed it so that we use a 8 prescaler which sets the compare match at 1/16th of a second.
-		You now count how many 16ths of a second you want until you trigger your actual interrupt. Works. Changed defines
-		for time so you can still call rtc_init with a scale but now it is defined in terms of actual time like second, quarter_second
-		etc. Read that section in the time.h file for more information. Tested and works, though the clock drifts more than
-		it used to
-	1.30.07 - Kevin
-		Modified the clock to run on timer3 on the Dragonfly. Works with decent accuracy. Using a prescaler of 256
-	the timer counts up to a precomputer value which will trigger an interrupt and reset the timer. Multiples of
-	256 change it by that multiple. Refer to the time.h file for all possible prescalers.
-		The interrupt will call a specified function _rtc_func every pulse.
-		All of it has been tested and it works.	
-		
-	10.29.07 - Andrew
-		Modify to accept multiple functions with different time intervals
-	
+  Change Log:
+  2.5.07 - Kevin
+  Aaron fixed the orb/servo code and made them use timer3 but compare registers B and C. He hard set the prescaler
+  to 8 so the RTC broke. Changed it so that we use a 8 prescaler which sets the compare match at 1/16th of a second.
+  You now count how many 16ths of a second you want until you trigger your actual interrupt. Works. Changed defines
+  for time so you can still call rtc_init with a scale but now it is defined in terms of actual time like second, quarter_second
+  etc. Read that section in the time.h file for more information. Tested and works, though the clock drifts more than
+  it used to
+  1.30.07 - Kevin
+  Modified the clock to run on timer3 on the Dragonfly. Works with decent accuracy. Using a prescaler of 256
+  the timer counts up to a precomputer value which will trigger an interrupt and reset the timer. Multiples of
+  256 change it by that multiple. Refer to the time.h file for all possible prescalers.
+  The interrupt will call a specified function _rtc_func every pulse.
+  All of it has been tested and it works.
 
 */
 #include <avr/interrupt.h>
@@ -29,14 +60,21 @@
 #include <util/delay.h>
 #include <time.h>
 #include <serial.h>
+
+/* Calculate how many cycles to delay for to get 1 ms. Based on F_CPU which should be defined by the makefile */
+#ifdef F_CPU
+#define WAIT_CYCLES ((F_CPU / 1000) / 10)
+#else
+#define WAIT_CYCLES (8000 / 10)
+#endif
 
+static volatile int _rtc_val = 0;
+static volatile int _rtc_pulse = 0;
+static volatile int _rtc_scale = 32;	//Defaults to 1 Second per pulse
+static void (*_rtc_f)(void) = 0;
+
+
 
-
-static volatile int _rtc_val[MAX_FUNCTIONS];
-static volatile int _rtc_pulse[MAX_FUNCTIONS];
-static volatile int _rtc_scale[MAX_FUNCTIONS];
-static int function_count = 0; //Number of functions
-static  void (*_rtc_f[MAX_FUNCTIONS])(void);
 /**
  * @defgroup time Time
  * @brief Time functions
@@ -48,18 +86,28 @@
 
 /**
  * Delays for the specified number of milliseconds.
- * The accuracy of this function is unknown.
+ * It depends on F_CPU to be defined in order to calculate how many cycles
+ * it should delay. If it is not defined, a default clock of 8MHz is assumed.
+ * 
+ * We use _delay_loop_2 which will run assembly instructions that should be
+ * 4 cycles long. Optimizations must be enabled for this to be true.
+ * That function is called to ensure around 1ms per execution. To generate
+ * multiple ms we run a for loop of how many milliseconds are desired.
+ *
+ * The error should be just the skew on the oscillator as the formula to 
+ * calculate delay cycles should always be a whole number. The is some skew
+ * in practice though it is unavoidable. Delaying for less than 1s should make
+ * the error negligable.
  *
  * @param ms the number of milliseconds to delay for
  **/
-void delay_ms(int ms) 
-{
-	for(; ms > 15; ms-=15)
-		_delay_ms(15);
-	_delay_ms(ms);
-}
+void delay_ms(int ms) {
+    for (; ms > 0; ms--) {
+        _delay_loop_2(WAIT_CYCLES);
+    }
+}
+
 
-
 /* 	Prescales defined in time.h. SECOND will give you 1 second.
 	More scales are defined in the time.h file.
 	rtc_func is the address to a function that you want called every clock tick. */
@@ -73,94 +121,40 @@
  * @param rtc_func the function called when the timer is triggered
  *
  * @see rtc_get, rtc_reset
+ *
  **/
- 
- int rtc_multi_init(int* prescale_opt,  void (*rtc_func[])(void), int argc) {
+void rtc_init(int prescale_opt, void (*rtc_func)(void)) {
 	
-	//Check that the number of functions doesn't exceed alloted space
-	if(argc > MAX_FUNCTIONS)
-		return -1;
+  //Clear timer register for Timer 3
+  TCNT3 = 0;
 	
-	function_count = argc;
-	//Clear timer register for Timer 3
-	TCNT3 = 0;
+  /* 	This sets the Waveform Generation Module to CTC (Clear Timer on Compare) Mode (100)
+	See page135 in Atmega128 Docs for more modes and explanations */
+  TCCR3B |= _BV(WGM32);
 	
-	/* 	This sets the Waveform Generation Module to CTC (Clear Timer on Compare) Mode (100)
-		See page135 in Atmega128 Docs for more modes and explanations */
-	TCCR3B |= _BV(WGM32);
+  /* 	This sets the prescaler for the system clock (8MHz) ie: divides the clock by some number.
+	Currently set to a prescaler of 8 because that is what the orb and servos use (they are on this timer as well)
+	See page137 in Atemga128 Docs for all the available prescalers */
+  TCCR3B |= _BV(CS31);
 	
-	/* 	This sets the prescaler for the system clock (8MHz) ie: divides the clock by some number.
-		Currently set to a prescaler of 8 because that is what the orb and servos use (they are on this timer as well)
-		See page137 in Atemga128 Docs for all the available prescalers */
-	TCCR3B |= _BV(CS31);
-	
-	/* 	Sets the two regsiters that we compare against. So the timer counts up to this number and
-		then resets back to 0 and calls the compare match interrupt.
-		8x10^6 / 8 = 1/16 Second. All values are based off of this number. Do not change it unless you
-		are l337*/
+  /* 	Sets the two regsiters that we compare against. So the timer counts up to this number and
+	then resets back to 0 and calls the compare match interrupt.
+	8x10^6 / 8 = 1/16 Second. All values are based off of this number. Do not change it unless you
+	are l337*/
 		
-	OCR3A = 0xF424;	
+  OCR3A = 0xF424;	
 
-	/* 	Enable Output Compare A Interrupt. When OCR3A is met by the timer TCNT3 this interrupt will be
-		triggerd. (See page140 in Atmega128 Docs for more information */
-	ETIMSK |= _BV(OCIE3A);
+  /* 	Enable Output Compare A Interrupt. When OCR3A is met by the timer TCNT3 this interrupt will be
+	triggerd. (See page140 in Atmega128 Docs for more information */
+  ETIMSK |= _BV(OCIE3A);
 	
-
-	/* Set number of functions*/
-	function_count = argc;
+  /*	Store the pointer to the function to be used in the interrupt */
+  _rtc_f = rtc_func;
 	
-	/*	Store how many 1/16ths of a second you want to let by before triggering an interrupt */
-	
-	for (int i = 0; i <argc; i++) {
-		_rtc_pulse[i] = 0;
-		_rtc_val[i] = 0;
-		_rtc_f[i] = rtc_func[i];
-		/*	Store how many 1/16ths of a second you want to let by before triggering an interrupt */
-		_rtc_scale[i] = prescale_opt[i];
-	}
-	return argc-1;
+  /*	Store how many 1/16ths of a second you want to let by before triggering an interrupt */
+  _rtc_scale = prescale_opt;
 }
 
-int rtc_add_function(void (*rtc_func) (void), int prescale_opt)
-{
-	if(function_count>=MAX_FUNCTIONS)
-		return -1;
-	_rtc_f[function_count] = rtc_func;
-	_rtc_pulse[function_count] = 0;
-	_rtc_scale[function_count]  = prescale_opt;
-	//return position in array that function was added to, increment count
-	//this position may change if functions are removed
-	return function_count++;
-}
-
-//remove function pointer
-int rtc_remove_function(void (*rtc_func)(void))
-{
-	for(int i = 0; i < function_count; i++)	
-		if(_rtc_f[i] == rtc_func) {
-			//If not last function replace with last and reduce number of functions
-			if(i < function_count-1) {
-				_rtc_f[i] = _rtc_f[function_count-1];
-				_rtc_pulse[i] = _rtc_pulse[function_count-1];
-				_rtc_scale[i] = _rtc_scale[function_count-1];
-			}
-			return --function_count;
-		}
-	
-	return -1;
-}
-
-int rtc_get_function_count()
-{
-	return function_count;
-}
-
-int rtc_init(int prescale_opt, void (*rtc_func)(void)){
-	int temp[] = {prescale_opt};
-	void (*temp2[MAX_FUNCTIONS])(void) = {rtc_func};
-	return rtc_multi_init(temp, temp2, 1);
-}
-
 /**
  * Returns the time elapsed in seconds since the last call to
  * rtc_init or rtc_reset.
@@ -169,52 +163,39 @@
  *
  * @see rtc_init, rtc_reset
  **/
- 
-int rtc_get(void){
-	return _rtc_val[0];
+int rtc_get(void) {
+  return _rtc_val;
 }
 
-int rtc_get_pos(int pos){
-	return _rtc_val[pos];
-}
-
 /**
  * Resets the real time clock counter to 0.
  *
  * @see rtc_init, rtc_get
  **/
- 
-void rtc_reset(void){
-	_rtc_val[0] = 0;
+void rtc_reset(void) {
+  _rtc_val = 0;
 }
 
-void rtc_reset_all(void)
-{
-	for(int i =0; i < function_count; i++)
-		_rtc_val[i] = 0;
-}
-
 /** @} **/ //end defgroup
 
 /*	Called every pulse. Function in _rtc_f is called every _rtc_scale and also the counter is updated.
 	Bascially, since the pulse is hard set at 1/16s  you want to count how many 16ths of a second have passed
 	and when it reaches the amount of time you want, execute the code. */
+SIGNAL(TIMER3_COMPA_vect) {
 
-SIGNAL(TIMER3_COMPA_vect) {
-	for(int functionNumber = 0; functionNumber < function_count; functionNumber++)
-	{
-		if (_rtc_pulse[functionNumber] ==  _rtc_scale[functionNumber]) {
-			//Increment the real time clock counter
-			_rtc_val[functionNumber]++;
-			
-			//Calls the function tied to the real time clock if defined
-			_rtc_f[functionNumber]();
-			
-			//Resets the pulse until the next scale is matched
-			_rtc_pulse[functionNumber] = 0;
-		}	
-		_rtc_pulse[functionNumber]++;
-	}
-	//Updates the amount of pulses seen since the last scale match
+  if (_rtc_pulse ==  _rtc_scale) {
+    //Increment the real time clock counter
+    _rtc_val++;
+		
+    //Calls the function tied to the real time clock if defined
+    if(_rtc_f != 0)
+      _rtc_f();
+		
+    //Resets the pulse until the next scale is matched
+    _rtc_pulse = 0;
+  }	
 	
+  //Updates the amount of pulses seen since the last scale match
+  _rtc_pulse++;
+	
 }
Index: branches/encoders/code/projects/libdragonfly/spi.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/spi.h	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/spi.h	(revision 1345)
@@ -4,6 +4,11 @@
  * @author Colony Project, CMU Robotics Club
  **/
 
+/**
+ * @addtogroup spi
+ * @{
+ **/
+
 #ifndef __SPI_H__
 #define __SPI_H__
 
@@ -21,7 +26,21 @@
 typedef void (*spi_fun_recv_t)(char);
 typedef void (*spi_fun_recv_complete_t)(void);
 
+/** 
+* @brief Initialize SPI
+* 
+* @param spi_fun_recv_t The function that handles SPI data, byte for byte.
+* @param spi_fun_recv_complete_t  Called on a completed transmission - typically for cleaning up.
+*/
 void spi_init (spi_fun_recv_t, spi_fun_recv_complete_t);
+
+/** 
+* @brief Initialize SPI transfer.
+* 
+* @param char The number of bytes to transfer.
+*/
 void spi_transfer (char);
 
+/**@}**/ //end group
+
 #endif
Index: branches/encoders/code/projects/libdragonfly/analog.c
===================================================================
--- branches/encoders/code/projects/libdragonfly/analog.c	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/analog.c	(revision 1345)
@@ -1,156 +1,346 @@
-/*
- * analog.c - Contains the function implementations for manipulating the ADC
- * on the firefly+ board
- * 
- * author:  CMU Robotics Club, Colony Project
- * code mostly taken from fwr analog file (author: Tom Lauwers)
- */
-
-#include <util/delay.h>
-#include <avr/interrupt.h>
-#include "analog.h"
-
-// Internal Function Prototypes
-void set_adc_mux(int which);
-
-/**
- * @defgroup analog Analog
- * Functions for manipulation the ADC on the dragonfly board.
- * All definitions may be found in analog.h.
- *
- * @{
- **/
-
-
-/**
- * Initializes the ADC.
- * Call analog_init before reading from the analog ports.
- *
- * @see analog8, analog10
- **/
-void analog_init(void)
-{
-	// ADC Status Register A
-	// Bit 7 - ADEN is set (enables analog)
-	// Bit 6 - Start conversion bit is set (must be done once for free-running mode)
-	// Bit 5 - Enable Auto Trigger (for free running mode)
-	// Bit 4 - ADC interrupt flag, 0
-	// Bit 3 - Enable ADC Interrupt (required to run free-running mode)
-	// Bits 2-0 - Set to create a clock divisor of 128, to make ADC clock = 8,000,000/128
-	ADCSRA |= 0xEF;
-
-	// ADC Status Register B
-	// Bit 7, 5-3 - Must be cleared
-	// Bit 2:0 - Set mode, currently cleared for free running operation
-	// Bit 6 - Analog comparator mode - cleared
-//	ADCSRB = 0x00;
-	
-	// ADMUX register
-	// Bit 7,6 - Set voltage reference to AVcc (0b01)
-	// Bit 5 - Set ADLAR bit for left adjust to do simple 8-bit reads
-	// Bit 4 - X
-	// Bit 3:0 - Sets the current channel, set to ADC7 (the external mux)
-	ADMUX = 0x67;
-
-	// Set external mux lines to outputs
-	DDRG |= 0x1C;
-	set_adc_mux(0x07);
-}	
-
-
-/**
- * Reads an eight bit number from an analog port.
- * analog_init must be called before using this function.
- * 
- * @param which the analog port to read from. One of
- * the constants AN0 - AN7.
- *
- * @return the eight bit input to the specified port
- *
- * @see analog_init, analog10
- **/
-unsigned int analog8(int which)
-{
-	if(which < EXT_MUX)
-		ADMUX = 0x60 + which;
-	else if(which == EXT_MUX)
-		return 0;
-	else
-	{
-		ADMUX = 0x60 + EXT_MUX;
-		set_adc_mux(which - 8);
-		_delay_ms(1);
-	}
-	
-	_delay_ms(1); // need at least 130 us between conversions
-	return ADCH;
-}
-
-/**
- * Reads a ten bit number from the specified port.
- * analog_init must be called before using this function.
- *
- * @param which the analog port to read from. Typically
- * a constant, one of AN0 - AN7.
- *
- * @return the ten bit number input to the specified port
- * 
- * @see analog_init, analog8
- **/
-unsigned int analog10(int which)
-{
-	unsigned int adc_h = 0;
-	unsigned int adc_l = 0;
-
-	if(which < EXT_MUX)
-		ADMUX = 0x60 + which;
-	else if(which == EXT_MUX)
-		return 0;
-	else
-	{
-		ADMUX = 0x60 + EXT_MUX;
-		set_adc_mux(which - 8);
-		_delay_ms(1);
-	}
-
-	_delay_ms(1);
-	adc_l = ADCL; /* highest 2 bits of ADCL -> least 2 bits of analog val */
-	adc_h = ADCH; /* ADCH -> 8 highest bits of analog val */
-	
-	return (adc_h << 2) | (adc_l >> 6);
-}
-
-/**
- * Returns the current position of the wheel, as an integer
- * in the range 0 - 255.
- * analog_init must be called before using this function.
- *
- * @return the orientation of the wheel, as an integer in
- * the range 0 - 255.
- *
- * @see analog_init
- **/
-int wheel(void)
-{
-	return analog8(WHEEL_PORT);
-}
-
-/**@}**/ //end defgroup
-
-SIGNAL (SIG_ADC)
-{
-	// This is just here to catch ADC interrupts because ADC is free running.  
-	// No code needs to be in here.
-}
-
-void set_adc_mux(int which)
-{
-  // FIX THIS IN NEXT REVISION
-  // ADDR2 ADDR1 ADDR0
-  // G2.G4.G3 set mux to port 0-7 via binary selection
-  // math would be much cleaner if it was G4.G3.G2
-  
-  // mask so only proper bits are possible.  
-  PORTG = (PORTG & 0xE3) | ((which & 0x03) << 3) | (which & 0x04);
-}
-
+/**
+ * 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 analog.c
+ * @brief Analog input and output
+ *
+ * Contains functions for manipulating the ADC on the Dragonfly board.
+ * 
+ * @author Colony Project, CMU Robotics Club
+ * code mostly taken from fwr analog file (author: Tom Lauwers)
+ **/
+
+#include <util/delay.h>
+#include <avr/interrupt.h>
+#include "analog.h"
+#include "serial.h"
+// Internal Function Prototypes
+void set_adc_mux(int which);
+
+/**
+ * @defgroup analog Analog
+ * Functions for manipulation the ADC on the dragonfly board.
+ * All definitions may be found in analog.h.
+ *
+ * @{
+ **/
+
+int adc_loop_running = 0;
+int adc_current_port = 0;
+adc_t an_val[11];
+
+/**
+ * Initializes the ADC.
+ * Call analog_init before reading from the analog ports.
+ *
+ * @see analog8, analog10, analog_get8, analog_get10
+ **/
+void analog_init(int start_conversion) {
+	for (int i = 0; i < 11; i++) {
+		an_val[i].adc10 = 0;
+		an_val[i].adc8 = 0;
+	}
+
+	// ADMUX register
+	// Bit 7,6 - Set voltage reference to AVcc (0b01)
+	// Bit 5 - ADLAR set to simplify moving from register
+	// Bit 4 - X
+	// Bit 3:0 - Sets the current channel
+	// Initializes to read from AN1 first (AN0 is reservered for the BOM)
+	ADMUX = 0;
+	ADMUX |= ADMUX_OPT | _BV(MUX0);
+
+	// ADC Status Register A
+	// Bit 7 - ADEN is set (enables analog)
+	// Bit 6 - Start conversion bit is set (must be done once for free-running mode)
+	// Bit 5 - Enable Auto Trigger (for free running mode) NOT DOING THIS RIGHT NOW
+	// Bit 4 - ADC interrupt flag, 0
+	// Bit 3 - Enable ADC Interrupt (required to run free-running mode)
+	// Bits 2-0 - Set to create a clock divisor of 128, to make ADC clock = 8,000,000/64 = 125kHz
+	ADCSRA = 0;
+	ADCSRA |= _BV(ADEN) | _BV(ADIE) | _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0);
+	
+	// Set external mux lines to outputs
+	DDRG |= 0x1C;
+	
+	// Set up first port for conversions
+	set_adc_mux(0x00);
+	adc_current_port = AN1;
+
+	//Start the conversion if requested
+	if (start_conversion)
+		analog_start_loop();
+	else
+		analog_stop_loop();
+}	
+
+/**
+ * Returns the 8-bit analog conversion of which from
+ * the lookup table. If the requested port is the BOM_PORT
+ * you will get an automatic 0 since the BOM_PORT is not
+ * read in the loop and not stored. If you need that port
+ * you should use the functions in bom.c. There is an analog_get8
+ * function which for instant lookups but should be avoided unless
+ * you know what you're doing.
+ *
+ * @param which the port that you want to read
+ *
+ * @bug may cause a seg fault if which is a larger value
+ * than exists in an_val table. Not sure if we should fix
+ * this or not since it would add overhead.
+ *
+ * @return 8-bit analog value for the which port requested
+ *
+ * @see analog10, analog_get8, analog_get10
+ **/
+unsigned int analog8(int which) {
+	if (which == BOM_PORT) {
+		return 0;
+	} else {
+		return an_val[which - 1].adc8;
+	}
+}
+
+/**
+ * Returns the 10-bit analog conversion of which from
+ * the lookup table. If the requested port is the BOM_PORT
+ * you will get an automatic 0 since the BOM_PORT is not
+ * read in the loop and not stored. If you need that port
+ * you should use the functions in bom.c. There is an analog_get10
+ * function which for instant lookups but should be avoided unless
+ * you know what you are doing.
+ *
+ * @param which the port that you want to read
+ *
+ * @bug may cause a seg fault if which is a larger value
+ * than exists in an_val table. Not sure if we should fix
+ * this or not since it would add overhead.
+ *
+ * @return 10-bit analog value for the which port requested
+ *
+ * @see analog8, analog_get8, analog_get10
+ **/
+unsigned int analog10(int which) {
+	if (which == BOM_PORT) {
+		return 0;
+	} else {
+		return an_val[which - 1].adc10;
+	}
+}
+
+/**
+ * Starts the analog update loop. Will continue to run
+ * until analog_stop_loop is called.
+ *
+ * @see analog_stop_loop
+ **/
+void analog_start_loop(void) {
+	//Start the conversion
+	ADCSRA |= _BV(ADSC);
+	adc_loop_running = 0x1;
+}
+
+/**
+ * Stops the analog update loop. If there is a current
+ * read, it will finish up and be stored before the loop
+ * is interrupted. No further updates will be made until
+ * the loop is started again.
+ *
+ * @see analog_start_loop
+ **/
+void analog_stop_loop(void) {
+	//Stop the conversion
+	adc_loop_running = 0x0;
+}
+/**
+ * Reads an 8-bit number from an analog port.
+ * analog_init must be called before using this function.
+ * The analog loop must also be stopped before using this
+ * function or you will mess up the lookup table. You
+ * must also reenabled the loop when you are done unless
+ * you are doing more instant reads. See analog_stop_loop
+ * and analog_start_loop for more information about the loop.
+ * 
+ * @param which the analog port to read from. One of
+ * the constants AN0 - AN7.
+ *
+ * @return the 8-bit input to the specified port
+ *
+ * @see analog_init, analog_get10, analog8, analog_stop_loop,
+ * analog_start_loop
+ **/
+unsigned int analog_get8(int which) {	
+	// Let any previous conversion finish
+	while (ADCSRA & _BV(ADSC));
+	
+	if(which < EXT_MUX) {
+		ADMUX = ADMUX_OPT + which;
+	} else {
+		ADMUX = ADMUX_OPT + EXT_MUX;
+		set_adc_mux(which - 8);
+	}
+	
+	// Start the conversion
+	ADCSRA |= _BV(ADSC);
+
+	// Wait for the conversion to finish
+	while (ADCSRA & _BV(ADSC));
+
+	return ADCH; //since we left aligned the data, ADCH is the 8 MSB.
+}
+
+/**
+ * Reads an 10-bit number from an analog port.
+ * analog_init must be called before using this function.
+ * The analog loop must also be stopped before using this
+ * function or you will mess up the lookup table. You
+ * must also reenabled the loop when you are done unless
+ * you are doing more instant reads. See analog_stop_loop
+ * and analog_start_loop for more information about the loop.
+ * 
+ *
+ * @param which the analog port to read from. Typically
+ * a constant, one of AN0 - AN7.
+ *
+ * @return the 10-bit number input to the specified port
+ * 
+ * @see analog_init, analog_get8, analog10, analog_stop_loop,
+ * analog_start_loop
+ **/
+unsigned int analog_get10(int which) {
+	int adc_h;
+	int adc_l;
+	
+	// Let any previous conversion finish
+	while (ADCSRA & _BV(ADSC));
+
+	if(which < EXT_MUX) {
+		ADMUX = ADMUX_OPT + which;
+	} else {
+		ADMUX = ADMUX_OPT + EXT_MUX;
+		set_adc_mux(which - 8);
+	}
+	
+	// Start the conversion
+	ADCSRA |= _BV(ADSC);
+
+	// Wait for the conversion to finish
+	while (ADCSRA & _BV(ADSC));
+	
+	adc_l = ADCL;
+	adc_h = ADCH;
+
+	return ((adc_h << 2) | (adc_l >> 6));
+}
+
+/**
+ * Returns the current position of the wheel, as an integer
+ * in the range 0 - 255.
+ * analog_init must be called before using this function.
+ *
+ * @return the orientation of the wheel, as an integer in
+ * the range 0 - 255.
+ *
+ * @see analog_init
+ **/
+int wheel(void) {
+	return analog8(WHEEL_PORT);
+}
+
+
+/**
+ * Sets the value of the external analog mux. Values are read
+ * 	on AN7 physical port. (AN8 - AN15 are "virtual" ports).
+ *
+ * @param which which analog mux port (0-7) which corresponds
+ * 		  to AN8-AN15.
+ *
+ * @bug FIX THIS IN THE NEXT BOARD REVISION:
+ *		ADDR2 ADDR1 ADDR0
+ *		G2.G4.G3 set mux to port 0-7 via vinary selection
+ *		math would be much cleaner if it was G4.G3.G2
+ *
+ * @see analog_init
+ **/
+void set_adc_mux(int which) {  
+  // mask so only proper bits are possible.  
+  PORTG = (PORTG & 0xE3) | ((which & 0x03) << 3) | (which & 0x04);
+}
+
+/**@}**/ //end defgroup
+
+
+ISR(ADC_vect) {
+	static volatile int adc_prev_loop_running = 0; 
+	int adc_h = 0;
+	int adc_l = 0;
+
+	//Store the value only if this read isn't for the BOM
+	if (ADMUX != BOM_PORT) {
+		adc_l = ADCL;
+		adc_h = ADCH;
+	
+		an_val[adc_current_port - 1].adc10 = (adc_h << 2) | (adc_l >> 6);
+		an_val[adc_current_port - 1].adc8 = adc_h;
+	}
+	
+	//Save the result only if we just turned off the loop
+	if (!adc_loop_running && !adc_prev_loop_running)
+		return;
+	
+	adc_prev_loop_running = adc_loop_running;
+	
+	//Skip AN7 because it is not a real port
+	if (adc_current_port == AN6) {
+		ADMUX = ADMUX_OPT | EXT_MUX;
+		set_adc_mux(AN8 - 8);
+		adc_current_port = AN8;
+	//Wrap around
+	} else if (adc_current_port == AN11) {
+		adc_current_port = AN1;
+		ADMUX = ADMUX_OPT | adc_current_port;
+	//Normal increment
+	} else {
+		adc_current_port++;
+	
+		if(adc_current_port < EXT_MUX) {
+			ADMUX = ADMUX_OPT | adc_current_port;
+		} else {
+			ADMUX = ADMUX_OPT | EXT_MUX;
+			set_adc_mux(adc_current_port - 8);
+		}
+	}
+
+	//Initiate next conversion only if we are running a loop
+	if (!adc_loop_running) {
+		return;
+    } else {
+    	ADCSRA |= _BV(ADSC);
+	}
+		
+	return;
+}
+
Index: branches/encoders/code/projects/libdragonfly/odometry.c
===================================================================
--- branches/encoders/code/projects/libdragonfly/odometry.c	(nonexistent)
+++ branches/encoders/code/projects/libdragonfly/odometry.c	(revision 1345)
@@ -0,0 +1,146 @@
+#include "odometry.h"
+#include <encoders.h>
+#include <math.h>
+#include <avr/interrupt.h>
+#include <dragonfly_lib.h>
+
+long lround(double d);
+
+
+int encoder_ltm1, encoder_rtm1;
+
+//Measure mm displacement in x and y from starting position. (+x:theta=0 +y:theta=M_PI/2)
+long diff_x,diff_y, velocity;
+
+//Measures radian angle displacement from starting position. (initially 0)
+double angle;
+
+/**
+ * Retrieve the estimated x position. [millimeters]
+ */
+long odometry_dx(void){
+	return diff_x;
+}
+
+/**
+ * Retrieve the estimated y position. [millimeters]
+ */
+long odometry_dy(void){
+	return diff_y;
+}
+
+/**
+ * Retrieve the estimated angle [radians]
+ */
+double odometry_angle(void){
+	return angle;
+}
+
+long odometry_velocity(void){
+	return velocity;
+}
+
+/**
+ * Initializes odometry to run on timer 2.
+ * Also resets all values so that the center of the robot is 
+ * considered to be at the origin facing the x direction.
+ */
+void odometry_init(void){
+	
+	encoders_init();
+	
+	delay_ms(100);
+
+	odometry_reset();
+
+	
+	//CTC Mode. CLK / 1024
+	TCCR2 = 0; // (Fully disconnected)
+	TCCR2 = _BV(WGM21) | _BV(CS22) | _BV(CS20); //CLK/1024 , CTC Mode.
+
+	TIMSK |= _BV(OCIE2);
+
+	OCR2 = ODOMETRY_CLK;
+}
+/**
+ * Resets all values so that the center of the robot is 
+ * considered to be at the origin facing the x direction.
+ */
+void odometry_reset(void){
+	diff_x = 0;
+	diff_y = 0;
+	encoder_ltm1 = encoder_read(LEFT);
+	encoder_rtm1 = encoder_read(RIGHT);
+	angle = 0.0;
+}
+
+
+void odometry_run(void){
+	//Angle swept through in a time step CCW-
+	double theta, rl, dc;
+	long dr,dl;
+	char buf[100];
+	
+	//Get the change in wheel positions
+	{	
+		int encoder_right = encoder_read(RIGHT);	
+		int encoder_left = encoder_read(LEFT);	
+	
+		dl = encoder_left - encoder_ltm1;
+		dr = encoder_right - encoder_rtm1;
+		
+		//No motion.
+		if(dl == 0 && dr == 0) return;
+
+		encoder_ltm1 = encoder_left;
+		encoder_rtm1 = encoder_right;
+	
+		// Try to avoid over/underflow.
+		dl = dl > 512 ? dl - 1024 :dl;
+		dl = dl < -512 ? dl + 1024 :dl;
+		dr = dr > 512 ? dr - 1024 :dr;
+		dr = dr < -512 ? dr + 1024 :dr;
+	
+		//Convert "clicks" to um
+		dl *= CLICK_DISTANCE_UM; //um
+		dr *= CLICK_DISTANCE_UM;
+	}
+	
+
+	if(dl == dr){
+		diff_x += lround(dl*cos(angle)/1000.0); //mm
+		diff_y += lround(dl*sin(angle)/1000.0); //mm
+		velocity = lround((dl * 1000.0)/(ODOMETRY_CLK*TIME_SCALE)); //um / ms = mm/s
+		sprintf(buf,"dc: %ld, velocity: %ld\r\n mm/s",dl,velocity);
+		usb_puts(buf);
+		return;
+	}
+	
+	//Call the left wheel 0, clockwise positive.
+	rl = ((double)(ROBOT_WIDTH_UM*dl))/((double)(dl - dr)); //um
+	theta = ((double)(dl - dr))/((double)ROBOT_WIDTH_UM);   //rad
+	
+	//Distance the center has traveled.
+	dc = (theta * (rl - ROBOT_WIDTH_UM)) / 2.0; //um
+	velocity = lround( dc * 1000 /(ODOMETRY_CLK*TIME_SCALE));
+	sprintf(buf,"dc: %lfum, velocity: %ld\r\n mm/s",dc,velocity);
+	usb_puts(buf);
+	
+	//angle is necessarily CCW+, so subtract.
+	angle -= ANGLE_SCALE * theta; 
+	
+	//Change state variables. Probably lose all measurements less then a mm.
+	diff_x += lround((DISTANCE_SCALE * dc * cos(angle))/1000.0); //mm
+	diff_y += lround((DISTANCE_SCALE * dc * sin(angle))/1000.0); //mm
+}
+
+ISR(TIMER2_COMP_vect){
+	odometry_run(); 
+}
+
+long lround(double d){
+	double f = floor(d);
+	return (long)(d - f > 0.5 ? f + 1 : f);
+}
+
+
Index: branches/encoders/code/projects/libdragonfly/encoders.c
===================================================================
--- branches/encoders/code/projects/libdragonfly/encoders.c	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/encoders.c	(revision 1345)
@@ -54,6 +54,7 @@
 	encoder_buf_index = 0;
 	left_data_buf = 0;
 	right_data_buf= 0;
+
 	left_data = -1;
 	right_data = -1;
 	
@@ -140,7 +141,37 @@
 	timecount = 0;
 }
 
+///////////////////////////////////////////////////////////////////////
+int left_data_at(int index) {
+	int tmp_idx = left_data_idx - index;
+	if (tmp_idx < 0)
+		tmp_idx += BUFFER_SIZE;
+	return left_data_array[tmp_idx];
+}
 
+int right_data_at(int index) {
+	int tmp_idx = right_data_idx - index;
+	if (tmp_idx < 0)
+		tmp_idx += BUFFER_SIZE;
+	return right_data_array[tmp_idx];	
+}
+
+
+int get_dx(char encoder, int index) {
+	int dx, ctr;
+	ctr = 0;
+	dx = 1024;
+	do {
+		if (encoder == LEFT)
+			dx = left_data_at(index+ctr+38) - left_data_at(index+ctr);
+		else
+			dx = right_data_at(index+ctr) - right_data_at(index+ctr+38);
+		ctr++;
+	} while ((dx > 30 || dx < -30) && ctr < 3);
+	if (dx > 30 || dx < -30)
+		return ERR_VEL;
+	return dx;
+}
 /** 
 * @brief Returns the approximated instantaneous velocity of the robot
 * in terms of encoder clicks.
@@ -150,6 +181,7 @@
 * @return The instantaneous velocity for the given wheel.
 */
 int encoder_get_v(char encoder){
+	/*
 	if (encoder == LEFT)
 		return left_data_array_bottom() - left_data_array_top();
 
@@ -156,10 +188,23 @@
 	if (encoder == RIGHT)
 		return right_data_array_top() - right_data_array_bottom();
 
-	return -1; /* TODO: velocity could be -1, use another value for error */
+	return -1;
+	*/
+	int vel1, vel2, tmp;
+	vel1 = get_dx(encoder, 0);
+	vel2 = get_dx(encoder, 1);
+
+	if (vel1 == ERR_VEL && vel2 == ERR_VEL)
+		return ERR_VEL << 1;
+	else if (vel2 == ERR_VEL)
+		return vel1 << 1;
+	else if (vel1 == ERR_VEL) 
+		return vel2 << 1;
+	else
+		return vel1 + vel2;
 }
+/////////////////////////////////////////////////////////////////////////
 
-
 /** 
 * @brief Waits until n encoder reads have occurred.
 * Counter is reset on functions exit.
Index: branches/encoders/code/projects/libdragonfly/lcd.c
===================================================================
--- branches/encoders/code/projects/libdragonfly/lcd.c	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/lcd.c	(revision 1345)
@@ -1,24 +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.
+ **/
 
-lcd-ar2.c - implementation for lcd functions
 
-Author: CMU Robotics Club, Colony Project
-
-This uses SPI.
-
-lcd_init MUST be called before anything can be used.
-
-*/
-
+/**
+ * @file lcd.c
+ * @brief LCD
+ *
+ * Implementation of functions for using the LCD.
+ *
+ * @author Colony Project, CMU Robotics Club
+ **/
 #include <avr/io.h>
 #include <lcd.h>
 #include <time.h>
 
-#define LCD_RS    PB4 // Command/Data
-#define LCD_RSTB  PE2 // reset line
-#define LCD_CS    PB0
-
-//////lcd defines
+//LCD defines
 #define RST _BV(4)  // pd4 (GPIO)
 #define SCE _BV(0)  // pb0 (~SS)
 #define D_C _BV(5)  // pd5 (GPIO?)
@@ -31,201 +49,15 @@
 #define LCDRESETPORT PORTD
 #define LCDRESETDDR DDRD
 
-// Internal Function Prototypes
-// inialize OLED and SPI
-void OLED_init(void);
-
-// reset Controller
-void Reset_SSD1339(void);
-
-// write command or data
-void write_c(unsigned char out_command);
-void write_d(unsigned char out_data);
-
-// these write data to the OLED on 8 bit data bus,  depends on MCU
-void LCD_out(unsigned char cmd);
-
-// these functions set / clear pins for OLED control lines.  they accecpt a 0 or 1 
-void DC(char stat);
-void RES(char stat);
-void CS(char stat);
-
 void lcd_putbyte(unsigned char b);
 
-void crazycircle (void) {
-/*  int i;
-  // draw 100 random circles
-  for(i = 0;i < 100;i++){
-      write_c(0x86);    // draw circle command
-      write_d(rand() % 130);
-      write_d(rand() % 130);
-      write_d(rand() % 64);
-      write_d(rand());
-      write_d(rand());
-      write_d(rand());
-      write_d(rand());
 
-      delay_ms(10);
-  }
-*/
-}
-
-void OLEDtest (void) {
-  //int   i = 0;
-
-  // Initialize
-  //Initialize();
-  OLED_init();
-
-  delay_ms(120);
-
-  write_c(0x8e);    // clear window command
-  write_d(0);
-  write_d(0);
-  write_d(130);
-  write_d(130);
-
-  delay_ms(100);
-  
-  write_c(0x92);    // fill enable command
-  write_d(0x01); 
-  
-  delay_ms(10);
-
-  crazycircle();
-/*
-  // write directly to ram,  this fills up bottom 1/3 of display with color pattern
-  write_c(0x5c);
-  for (i = 0; i < 2000; i++){
-  write_c(0x5c);  
-   write_d(i);
-   write_d(i);
-   write_d(i);
-  }
-  */
-} 
-
-/**********************************************************
-                      Initialize
-**********************************************************/
-
-void OLED_init(void)
-{
-  // Setup SPI here
-  SPCR = 0x5D; //  enable SPI, master, SPI mode 3
-  DDRB |= 0x06; // enable MOSI and SCK as outputs
-
-  LCD_out(0);
-  DC(0);
-  CS(0);
-  Reset_SSD1339();
-  write_c(0xa0); // Set Re-map / Color Depth
-  write_d(0x34);//0xb4); // 262K 8bit R->G->B
-  write_c(0xa1); // Set display start line
-  write_d(0x00); // 00h start
-  //write_c(0xa2); // Set display offset
-  //write_d(0x80); // 80h start
-  write_c(0xA6); // Normal display
-  write_c(0xad); // Set Master Configuration
-  write_d(0x8e); // DC-DC off & external VcomH voltage & external pre-charge voltage
-  write_c(0xb0); // Power saving mode
-  write_d(0x05);
-  write_c(0xb1); // Set pre & dis_charge
-  write_d(0x11); // pre=1h dis=1h
-  write_c(0xb3); // clock & frequency
-  write_d(0xf0); // clock=Divser+1 frequency=fh
-  write_c(0xbb); // Set pre-charge voltage of color A B C
-  write_d(0xff); // color A  was 1c
-  write_d(0xff); // color B  was 1c
-  write_d(0xff); // color C  was 1c
-  write_c(0xbe); // Set VcomH
-  write_d(0x1f); //
-  write_c(0xc1); // Set contrast current for A B C
-  write_d(0xCa); // Color A was AA
-  write_d(0xD4); // Color B was B4
-  write_d(0xF8); // Color C was C8
-  write_c(0xc7); // Set master contrast
-  write_d(0x0f); // no change
-  write_c(0xca); // Duty
-  write_d(0x7f); // 127+1
-  write_c(0xaf); // Display on
-}
-
-void Reset_SSD1339(void)
-{
-  RES(0);
-  delay_ms(100);
-  RES(1);
-}
-void write_c(unsigned char out_command)
-{
-  DC(0);CS(0);
-  //delay_ms(1);
-  
-  LCD_out(out_command);
- // delay_ms(1);
-  
-  DC(1);
-  // potentially add NOP
-  CS(1);
-
-}
-
-void write_d(unsigned char out_data)
-{
-  DC(1);CS(0);
- // delay_ms(1);
- 
-  LCD_out(out_data);
- // delay_ms(1);
-  DC(1);
-  //potentially add NOP
-  CS(1);
-}
-
-// these functions set / clear pins for LCD control lines.  they accecpt a 0 or 1 
-void DC(char stat)
-{
-	DDRB |= _BV(LCD_RS);				// RS is P0.30, set to output
-	if (stat)	
-		PORTB |= _BV(LCD_RS);
-	else
-		PORTB &= ~(_BV(LCD_RS));
-}
-
-void RES(char stat)
-{
-	DDRE |= _BV(LCD_RSTB);				// RSTB is P0.7, set to output
-	if (stat)	
-		PORTE |= _BV(LCD_RSTB);
-	else
-		PORTE &= ~(_BV(LCD_RSTB));
-}
-
-void CS(char stat)
-{
-	DDRB |= _BV(LCD_CS);				// RSTB is P0.7, set to output
-	if (stat)	
-		PORTB |= _BV(LCD_CS);
-	else
-		PORTB &= ~(_BV(LCD_CS));
-}
-
-// send to the LCD
-void LCD_out(unsigned char cmd)
-{
-	SPDR = cmd;
-    // could also do this via interrupts
-	loop_until_bit_is_set(SPSR, SPIF);
-}
-
-
 //**************************************Old shit below!*****************
 /*
-FontLookup - a lookup table for all characters
+  FontLookup - a lookup table for all characters
 */
 static const unsigned char FontLookup [][5] =
-{
+  {
     { 0x00, 0x00, 0x00, 0x00, 0x00 },  // sp
     { 0x00, 0x00, 0x5f, 0x00, 0x00 },   // !
     { 0x00, 0x07, 0x00, 0x07, 0x00 },   // "
@@ -322,7 +154,7 @@
     { 0x41, 0x41, 0x36, 0x08, 0x00 },   // }
     { 0x02, 0x01, 0x01, 0x02, 0x01 },   // ~
     { 0x55, 0x2A, 0x55, 0x2A, 0x55 },   // del
-};
+  };
 
 
 /**
@@ -338,33 +170,32 @@
  * Initializes the LCD. Must be called before any other
  * LCD functions.
  **/
-void lcd_init(void)
-{
-	LCDDDR |= (SCE | SDI | SCK);
-	LCDRESETDDR |= (RST|D_C);
+void lcd_init(void) {
+  LCDDDR |= (SCE | SDI | SCK);
+  LCDRESETDDR |= (RST|D_C);
 
-	LCDPORT &= ~( SCE | SDI | SCK);
-	LCDRESETPORT &=~(D_C);
+  LCDPORT &= ~( SCE | SDI | SCK);
+  LCDRESETPORT &=~(D_C);
   
-	SPCR |= 0x50;//0b01010000; // no SPI int, SPI en, Master, sample on rising edge, fosc/2
-	SPSR |= 0x01;       // a continuation of the above
+  SPCR |= 0x50;//0b01010000; // no SPI int, SPI en, Master, sample on rising edge, fosc/2
+  SPSR |= 0x01;       // a continuation of the above
 
-	LCDRESETPORT |= RST;
-	delay_ms(10);
-	LCDRESETPORT &= (~RST);
-	delay_ms(100);
-	LCDRESETPORT |= RST;
+  LCDRESETPORT |= RST;
+  delay_ms(10);
+  LCDRESETPORT &= (~RST);
+  delay_ms(100);
+  LCDRESETPORT |= RST;
 	
-    lcd_putbyte( 0x21 );  // LCD Extended Commands.
-    lcd_putbyte( 0xC8 );  // Set LCD Vop (Contrast).
-    lcd_putbyte( 0x06 );  // Set Temp coefficent.
-    lcd_putbyte( 0x13 );  // LCD bias mode 1:48.
-    lcd_putbyte( 0x20 );  // LCD Standard Commands, Horizontal addressing mode.
-    lcd_putbyte( 0x0C );  // LCD in normal mode.
+  lcd_putbyte( 0x21 );  // LCD Extended Commands.
+  lcd_putbyte( 0xC8 );  // Set LCD Vop (Contrast).
+  lcd_putbyte( 0x06 );  // Set Temp coefficent.
+  lcd_putbyte( 0x13 );  // LCD bias mode 1:48.
+  lcd_putbyte( 0x20 );  // LCD Standard Commands, Horizontal addressing mode.
+  lcd_putbyte( 0x0C );  // LCD in normal mode.
 	
-	LCDRESETPORT |= D_C;		//put it in init instead of main
+  LCDRESETPORT |= D_C;		//put it in init instead of main
 	
-	lcd_clear_screen();
+  lcd_clear_screen();
 }
 
 /**
@@ -373,11 +204,11 @@
  * @see lcd_init
  **/
 void lcd_clear_screen( void ) {
-	int i;
-	for (i = 0; i < 504; i++)
-			lcd_putbyte(0x0);
+  int i;
+  for (i = 0; i < 504; i++)
+    lcd_putbyte(0x0);
 	
-	lcd_gotoxy(0,0);
+  lcd_gotoxy(0,0);
 }
 
 /**
@@ -388,37 +219,35 @@
  *
  * @see lcd_init
  **/
-void lcd_putc(char c)
-{
-	int i;
+void lcd_putc(char c) {
+  int i;
 	
-	for (i = 0; i < 5; i++)
-		lcd_putbyte(FontLookup[c-32][i]);
-	lcd_putbyte(0);
+  for (i = 0; i < 5; i++)
+    lcd_putbyte(FontLookup[c-32][i]);
+  lcd_putbyte(0);
 }
 
 /*
-print an entire string to the lcd
+  print an entire string to the lcd
 */
-void lcd_puts(char *s)
-{
-	char *t = s;
-	while (*t != 0)
-	{
-		lcd_putc(*t);
-		t++;
-	}
+void lcd_puts(char *s) {
+  char *t = s;
+  while (*t != 0) {
+    lcd_putc(*t);
+    t++;
+  }
 }
 
 /*
-go to coordinate x, y
-y: vertically - 1 char
-x: horizontally - 1 pixel
+  go to coordinate x, y
+  y: vertically - 1 char
+  x: horizontally - 1 pixel
 
-multiply x by 6 if want to move 1 entire character
+  multiply x by 6 if want to move 1 entire character
 
-origin (0,0) is at top left corner of lcd screen
+  origin (0,0) is at top left corner of lcd screen
 */
+
 /**
  * Move the current cursor position to the one specified.
  * lcd_init must be called before this function may be used.
@@ -428,8 +257,7 @@
  *
  * @see lcd_init
  **/
-void lcd_gotoxy(int x, int y)
-{
+void lcd_gotoxy(int x, int y) {
   LCDRESETPORT &= ~(D_C);
   lcd_putbyte(0x40 | (y & 0x07));
   lcd_putbyte(0x80 | (x & 0x7f));
@@ -437,10 +265,11 @@
 }
 
 /*
-prints an int to the lcd
+  prints an int to the lcd
 
-code adapted from Chris Efstathiou's code (hendrix@otenet.gr)
+  code adapted from Chris Efstathiou's code (hendrix@otenet.gr)
 */
+
 /**
  * Print an integer to the LCD screen.
  * lcd_init must be called before this function may be used.
@@ -450,28 +279,30 @@
  * @see lcd_init
  **/
 void lcd_puti(int value ) {
-	unsigned char lcd_data[6]={'0','0','0','0','0','0' }, position=sizeof(lcd_data), radix=10; 
+  unsigned char lcd_data[6]={'0','0','0','0','0','0' }, position=sizeof(lcd_data), radix=10; 
 
-        /* convert int to ascii  */ 
-        if(value<0) { lcd_putc('-'); value=-value; }    
-        do { position--; *(lcd_data+position)=(value%radix)+'0'; value/=radix;  } while(value); 
+  /* convert int to ascii  */ 
+  if(value<0) { 
+    lcd_putc('-'); 
+    value=-value; 
+  }    
+  do { 
+    position--; 
+    *(lcd_data+position)=(value%radix)+'0'; 
+    value/=radix;  
+  } while(value); 
 
     
-        /* start displaying the number */
-        for(;position<=(sizeof(lcd_data)-1);position++)
-          {
-            
-            lcd_putc(lcd_data[position]);
-          }
+  /* start displaying the number */
+  for(;position<=(sizeof(lcd_data)-1);position++)
+    lcd_putc(lcd_data[position]);
 
-	return;
+  return;
 }
 
 /** @} **/ //end defgroup
 
-void lcd_putbyte(unsigned char b)
-{
-	SPDR = b;
-	while (!(SPSR & 0x80)); /* Wait until SPI transaction is complete */
+void lcd_putbyte(unsigned char b) {
+  SPDR = b;
+  while (!(SPSR & 0x80)); /* Wait until SPI transaction is complete */
 }
-
Index: branches/encoders/code/projects/libdragonfly/time.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/time.h	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/time.h	(revision 1345)
@@ -1,4 +1,30 @@
 /**
+ * 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 time.h
  * @brief Contains time-related functions and definitions
  *
@@ -11,10 +37,6 @@
 #ifndef _TIME_H_
 #define _TIME_H_
 
-/*Maximum # of functions*/
-#define MAX_FUNCTIONS 10
-
-
 /*	Predefined times for prescale_opt in time.c.
 	To make you own, know that a pulse is 1/16th of a second. You cannot get less than this. To get more, you need
 	to know how many 16ths of a second are in the time you want. (Time_desired * 16 = prescaler_opt)
@@ -41,19 +63,12 @@
 /** @brief Delay execution for the specified time **/
 void delay_ms(int ms) ;
 /** @brief Enable the realtime clock **/
-int rtc_multi_init(int* prescale_opt, void (*rtc_fun[])(void), int argc);
+void rtc_init(int prescale_opt, void (*rtc_func)(void));
 /** @brief Reset the counter of the realtime clock **/
 void rtc_reset(void);
 /** @brief Get the value of the realtime clock. **/
 int rtc_get(void);
 
-int rtc_add_function(void (*rtc_f) (void), int prescale_opt);
-
-int rtc_remove_function(void (*rtc_func)(void));
-int rtc_get_function_count(void);
-
-int rtc_init(int prescale_opt, void (*rtc_func)(void));
-
 /** @} **/
 
 #endif
Index: branches/encoders/code/projects/libdragonfly/analog.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/analog.h	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/analog.h	(revision 1345)
@@ -1,5 +1,30 @@
 /**
+ * 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 analog.h
  * @brief Contains functions and definitions for using the ADC
  * 
@@ -12,11 +37,13 @@
 	
  * @author Colony Project, CMU Robotics Club, based on firefly
  * code by Tom Lauwers
-*/
+ */
 
 #ifndef _ANALOG_H
 #define _ANALOG_H
 
+#include <inttypes.h>
+
 /**
  * @addtogroup analog
  * @{
@@ -43,18 +70,20 @@
 /** @brief Analog port 9 **/
 #define AN9 0x09
 /** @brief Analog port 10 **/
-#define AN10 0x0a
+#define AN10 0x0A
 /** @brief Analog port 11 **/
-#define AN11 0x0b
+#define AN11 0x0B
 /** @brief Analog port 12 **/
-#define AN12 0x0c
+#define AN12 0x0C
 /** @brief Analog port 13 **/
-#define AN13 0x0d
+#define AN13 0x0D
 /** @brief Analog port 14 **/
-#define AN14 0x0e
+#define AN14 0x0E
 /** @brief Analog port 15 **/
-#define AN15 0x0f
+#define AN15 0x0F
 
+/** @brief BOM_PORT analog port for BOM **/
+#define BOM_PORT AN0
 /** @brief EXT_MUX analog port **/
 #define EXT_MUX AN7
 /** @brief Analog port for the wheel **/
@@ -62,15 +91,38 @@
 /** @brief Analog port for the battery voltage detector **/
 #define BATT_PORT  AN11
 
-/** @brief Initialize analog ports. **/
-void analog_init(void);
-/** @brief Read an 8-bit number from an analog port. **/
+#define ADC_START 1
+#define ADC_STOP 0
+
+#define ADMUX_OPT 0x60
+
+/** @brief Struct to hold the value of a particular analog port **/
+typedef struct {
+  uint8_t adc8;
+  uint16_t adc10;
+} adc_t;
+
+
+/** @brief Initialize analog ports. Will start running a loop
+    if start_conversion is ADC_START.**/
+void analog_init(int start_conversion);
+/** @brief starts the analog loop. Doesn't do anything if the loop is already running. **/
+void analog_start_loop(void);
+/** @brief Stops the analog loop. Doesn't do anything if the loop is already stopped. **/
+void analog_stop_loop(void);
+/** @brief Returns an 8-bit analog value from the look up table. Use this instead of analog_get8. **/
 unsigned int analog8(int which);
-/** @brief Read a 10-bit number from an analog port. **/
+/** @brief Returns an 10-bit analog value from the look up table. Use this instead of analog_get10. **/
 unsigned int analog10(int which);
 /** @brief Read the position of the wheel. **/
 int wheel(void);
+/** @brief Read an 8-bit number from an analog port. Loop must be stopped for this to work. **/
+unsigned int analog_get8(int which);
+/** @brief Read a 10-bit number from an analog port. Loop must be stopped for this to work. **/
+unsigned int analog_get10(int which);
 
+
 /**@}**/ //end group
 
 #endif
+
Index: branches/encoders/code/projects/libdragonfly/move.c
===================================================================
--- branches/encoders/code/projects/libdragonfly/move.c	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/move.c	(revision 1345)
@@ -1,3 +1,38 @@
+/**
+ * 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 move.c
+ * @brief Functions for moving
+ *
+ * Implementation of functions for moving the robot.
+ *
+ * @author Colony Project, CMU Robotics Club
+ **/
+
 #include "dragonfly_lib.h"
 
 #include "move.h"
@@ -8,8 +43,7 @@
 void translateAngulartoLinear (int velocity, int omega, int* vl, int* vr);
 
 // global varaibles for move_avoid
-int d1, d2, d3, d4, d5;
-
+int d1, d2, d3, d4, d5; 
 /**
  * @defgroup move Movement
  * @brief Functions fo controlling robot motion
@@ -70,7 +104,7 @@
  *
  * @see motors_init, range_init, move
  **/
-void move_avoid(int velocity, int omega, int strength){
+void move_avoid(int velocity, int omega, int strength) {
   int pControl;
   int vl = 0;
   int vr = 0;
@@ -92,12 +126,12 @@
   d5=(temp == -1) ? d5 : temp;
   
   /* Avoid obstacles ahead
-  if(d2>170)
-    v*=-1;
+     if(d2>170)
+     v*=-1;
     
-  Naturally slow down if there is something in the way.
-  if(d2>150 || d1>180 || d3>180){
-    v>>=1;
+     Naturally slow down if there is something in the way.
+     if(d2>150 || d1>180 || d3>180){
+     v>>=1;
   */
   
   //pControl= (((d3-d1) + (d4-d5))*strength)/100;
@@ -120,9 +154,9 @@
 /**@}**///end the motion group
 
 void translateAngulartoLinear (int velocity, int omega, int* vl, int* vr) {
-	//omega: angle measure, positive couter-clockwise from front.
-	// -180 <= omega <= 180
-	//velocity: -255 <= velocity <= 255
+  //omega: angle measure, positive couter-clockwise from front.
+  // -180 <= omega <= 180
+  //velocity: -255 <= velocity <= 255
   
   long int vltemp, vrtemp;
   
@@ -130,17 +164,17 @@
   if (velocity < -255 || velocity >255 || omega < -255 || omega > 255) return;
 	
   //compute
-	vrtemp = velocity + omega * 3;
-	vltemp = velocity - omega * 3;
+  vrtemp = velocity + omega * 3;
+  vltemp = velocity - omega * 3;
 	
-	//check to see if max linear velocities have been exceeded.
+  //check to see if max linear velocities have been exceeded.
   if (vrtemp > 255) {
     vltemp = 255 * vltemp / vrtemp;
     vrtemp = 255;
   }
   if (vltemp > 255) {	   
-	vrtemp = 255 * vrtemp / vltemp;    
-	vltemp = 255;  
+    vrtemp = 255 * vrtemp / vltemp;    
+    vltemp = 255;  
   }
   if (vrtemp < -255) {
     vltemp = -255 * vltemp / vrtemp;
@@ -153,8 +187,4 @@
   
   *vr = (int)vrtemp;
   *vl = (int)vltemp;
-	
-
 }
-
-
Index: branches/encoders/code/projects/libdragonfly/odometry.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/odometry.h	(nonexistent)
+++ branches/encoders/code/projects/libdragonfly/odometry.h	(revision 1345)
@@ -0,0 +1,56 @@
+
+/**
+ * @file odometry.h
+ * @brief Code for estimating the robots pose.
+ * 
+ * Offers simple position and orientation information.
+ *
+ * @author Colony Project, CMU Robotics Club
+ **/
+
+#ifndef __ODOMETRY_C__
+#define __ODOMETRY_C__
+
+/**
+ * @addtogroup odometry 
+ * @{
+ **/
+
+//Odometry resolution, *64 microseconds.
+#define ODOMETRY_CLK 255u 
+#define TIME_SCALE 64
+
+//Wheel = 2.613 in.  
+//Circumference = 208.508133 mm
+//Distance per encoder click (circumference / 1024)  = 203.621224 um.
+//Robot width = 5.3745 in. = 136.5123 mm
+
+#define ROBOT_WIDTH_UM 137000  //um
+#define CLICK_DISTANCE_UM 204 //um
+
+#define DISTANCE_SCALE 2.10526316 //Magic constant.
+#define ANGLE_SCALE 1.12823207 //Magic constant.
+
+/** @brief Retrieve the robots estimated x position*/
+long odometry_dx(void);
+
+/** @brief Retrieve the robots estimated y position*/
+long odometry_dy(void);
+
+/** @brief Retrieve the robots estimated orientation*/
+double odometry_angle(void);
+
+/** @brief Initialize odometry. MUST be called before 
+ * the other functions work.**/
+void odometry_init(void);
+
+/** @brief Reset position and orientation to the origin facing
+ * the x axis.*/
+void odometry_reset(void);
+
+/** @brief Report estimated velocity [mm/s].*/
+long odometry_velocity(void);
+
+/**@}**/ //end group
+
+#endif
Index: branches/encoders/code/projects/libdragonfly/encoders.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/encoders.h	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/encoders.h	(revision 1345)
@@ -46,8 +46,13 @@
 #define MagINCn _BV(1)
 #define MagDECn _BV(0)
 
-#define BUFFER_SIZE 23
+//////////////////////////////
+//#define BUFFER_SIZE 23
+#define BUFFER_SIZE		46
+#define ERR_VEL			1024
+//////////////////////////////
 
+
 /** @brief Initialize encoders. **/
 void encoders_init(void);
 /** @brief Read instantaneous encoder value. **/
Index: branches/encoders/code/projects/libdragonfly/lcd.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/lcd.h	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/lcd.h	(revision 1345)
@@ -1,4 +1,30 @@
 /**
+ * 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 lcd.h
  * @brief Contains definitions for dealing with the LCD screen.
  * 
@@ -32,3 +58,4 @@
 /** @} **/
 
 #endif
+
Index: branches/encoders/code/projects/libdragonfly/battery.c
===================================================================
--- branches/encoders/code/projects/libdragonfly/battery.c	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/battery.c	(revision 1345)
@@ -1,3 +1,38 @@
+/**
+ * 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 battery.c
+ * @brief Implementation for reading battery level
+ *
+ * Contains functions for reading the battery level.
+ *
+ * @author Colony Project, CMU Robotics Club
+ **/
+
 #include "battery.h"
 #include "analog.h"
 
@@ -67,7 +102,8 @@
   
   int batt_reading = battery8();
 
-  if(next_read == 0) {
+  if(next_read == 0) 
+  {
     if(batt_reading > BATTERY_LOWV)
     {
         ret = 0;
@@ -96,7 +132,7 @@
  * for convenience. analog_init must be called before this
  * function may be used.
  *
- * @param n_samples The number of times to sample the battery voltage
+ * @param n_samples the number of times to sample the battery voltage
  *
  * @return the average reading for the battery voltage, where 128
  * is approximately 5 Volts.
@@ -108,7 +144,8 @@
   int i;
   long int sum = 0;
   
-  for(i = 0; i < n_samples; i++) {
+  for(i = 0; i < n_samples; i++) 
+  {
     sum += battery8();
   }
   
Index: branches/encoders/code/projects/libdragonfly/move.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/move.h	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/move.h	(revision 1345)
@@ -1,4 +1,30 @@
 /**
+ * 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 move.h
  * @brief Contains definitions for controlling robot motion
  * 
Index: branches/encoders/code/projects/libdragonfly/dio.c
===================================================================
--- branches/encoders/code/projects/libdragonfly/dio.c	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/dio.c	(revision 1345)
@@ -1,7 +1,38 @@
-/* 
-dio.c
-*/
+/**
+ * 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 dio.c
+ * @brief Digital Input and Output
+ *
+ * Implementation of functions for digital input and output.
+ *
+ * @author Colony Project, CMU Robotics Club
+ **/
+
 #include <avr/interrupt.h>
 #include <dio.h>
 #include <time.h>
@@ -41,11 +72,11 @@
  *
  * @return 1 or 0, depending on the value of the portpin.
  **/
-int digital_input(int portpin){
+int digital_input(int portpin) {
   int pin = portpin & 0x7;
   int pin_val = 0;
   
-  switch(portpin >> 3){
+  switch(portpin >> 3) {
   case _PORT_A:
     DDRA &= ~_BV(pin);
     pin_val = PINA;
@@ -67,7 +98,7 @@
     pin_val = PINE;
     return (pin_val >> pin) & 1;
   case _PORT_F:
-    if(pin>=4){
+    if(pin>=4) {
       MCUSR|=1<<7;
       MCUSR|=1<<7;
     }
@@ -163,7 +194,7 @@
       PORTG &= (0XFF - _BV(pins));
       break;
     }
-  }else { /* ( val == 1) */ 
+  } else { /* ( val == 1) */ 
     switch(portpin >> 3) {
     case _PORT_A:
       DDRA |= _BV(pins);
@@ -204,14 +235,12 @@
  *
  * @see button1_wait, button1_click
  **/
-int button1_read( void )
-{
-	int pin_val;
-	DDRG &= ~_BV(PING0);
-	PORTG|= _BV(PING0);
-    pin_val = PING;
-	return !((pin_val & _BV(PING0)));
-  
+int button1_read( void ) {
+  int pin_val;
+  DDRG &= ~_BV(PING0);
+  PORTG|= _BV(PING0);
+  pin_val = PING;
+  return !((pin_val & _BV(PING0)));
 }
 
 /**
@@ -219,8 +248,7 @@
  *
  * @see button1_read, button1_click
  **/
-void button1_wait( void )
-{
+void button1_wait( void ) {
   while(!button1_read() ) {
     delay_ms(15);
   }
@@ -234,12 +262,11 @@
  *
  * @see button1_read, button1_wait
  **/
-int button1_click()
-{
-  if(button1_read()){
+int button1_click() {
+  if(button1_read()) {
     while(button1_read());
     return 1;
-  }else{
+  } else {
     return 0;
   }
 }
@@ -251,13 +278,12 @@
  *
  * @see button2_wait, button2_click
  **/
-int button2_read( void )
-{
-	int pin_val;
-	DDRG &= ~_BV(PING1);
-	PORTG|= _BV(PING1);
-    pin_val = PING;
-	return !((pin_val & _BV(PING1)));
+int button2_read( void ) {
+  int pin_val;
+  DDRG &= ~_BV(PING1);
+  PORTG|= _BV(PING1);
+  pin_val = PING;
+  return !((pin_val & _BV(PING1)));
 }
 
 /**
@@ -265,8 +291,7 @@
  *
  * @see button2_read, button2_click
  **/
-void button2_wait( void )
-{
+void button2_wait( void ) {
   while(!button2_read()){
     delay_ms(15);
   }
@@ -280,13 +305,12 @@
  *
  * @see button2_read, button2_wait
  **/
-int button2_click()
-{
-	if(button2_read()){
+int button2_click() {
+  if(button2_read()) {
     while(button2_read());
     return 1;
-  }else{
-		return 0;
+  } else {
+    return 0;
   }
 }
 
Index: branches/encoders/code/projects/libdragonfly/bom.c
===================================================================
--- branches/encoders/code/projects/libdragonfly/bom.c	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/bom.c	(revision 1345)
@@ -1,14 +1,51 @@
+/**
+ * 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 bom.c
+ * @brief Implementation for using the BOM
+ *
+ * Contains functions for using the Bearing and Orientation Module (BOM)
+ *
+ * @author Colony Project, CMU Robotics Club
+ **/
+
 #include <dragonfly_lib.h>
 #include "bom.h"
 #include "dio.h"
+#include "serial.h"
 #include "analog.h"
 
-//constants
-const int lookup[16] = {7,6,5,0xe,1,4,3,2,0xf,0,0xd,8,0xc,0xb,9,0xa};
+//On the original BOM1.0, the emmitter angular order does not match the analog mux order
+//so you need to iterate through the mux index in the following order if you want to get
+//the detector readings in order:
+static const char lookup[16] = {7,6,5,0xe,1,4,3,2,0xf,0,0xd,8,0xc,0xb,9,0xa};
 
 // internal function prototypes
-void output_high(int which);
-void output_low(int which);
+static void bom_select(char which);
 
 /*
  Bk R Y (Analog)
@@ -43,8 +80,24 @@
 #define MONK2 _PIN_E7     //white
 
 #define BOM_VALUE_THRESHOLD 200
+#define NUM_BOM_LEDS 16
 
+/*
+  *The following pin definitions are for the BOM v1.5
+  */
 
+#define BOM_MODE	_PIN_E2	//dio0
+#define BOM_STROBE	_PIN_E3	//dio1
+
+#define BOM_DATA	_PIN_A0 //servo0
+#define BOM_CLOCK	_PIN_A1	//servo1
+
+#define BOM_S0		_PIN_E5	//dio3
+#define BOM_S1		_PIN_E4	//dio2
+#define BOM_S2		_PIN_E7	//dio4
+#define BOM_S3		_PIN_E6	//dio5
+#define BOM_OUT		PF0		//analog(yellow)
+
 /**
  * @defgroup bom BOM (Bearing and Orientation Module)
  * @brief Functions for dealing with the BOM.
@@ -60,8 +113,147 @@
  * @{
  **/
 
+static unsigned int bom_val[NUM_BOM_LEDS];
+static volatile char bom_type = BOM;
+static int select_pins[4];
+static int analog_pin;
+
 /**
- * Returns the direction of the maximum BOM reading,
+ * Initializes the BOM.
+ * Call bom_init before reading bom values or turning bom leds.
+ *
+ * @bugs INCOMPLETE - No utilization of BOM1.5 RSSI capability. Probably leave this out
+ * until Cornell and Pras return
+ * 
+ * @see bom_refresh, bom_leds_on, bom_leds_off
+ **/
+void bom_init(char type) {
+    bom_type = type;
+    
+    switch(bom_type) {
+    case BOM10:
+		select_pins[0] = MONK0; 
+		select_pins[1] = MONK1;
+		select_pins[2] = MONK2;
+		select_pins[3] = MONK3;
+		analog_pin = MONKI;
+        break;
+    case BOM15:
+        //Sets BOM1.5 to normal [BOM] mode
+        digital_output(BOM_MODE, 0);
+		select_pins[0] = BOM_S0; 
+		select_pins[1] = BOM_S1;
+		select_pins[2] = BOM_S2;
+		select_pins[3] = BOM_S3;
+		bom_set_leds(BOM_ALL);
+		analog_pin = BOM_OUT;
+        break;
+    case RBOM:
+        break;
+    //default:
+    }
+}
+
+/**
+ * Iterates through each bit in the bit_field. For each set bit, sets the corresponding bom select bits
+ *    and updates the corresponding bom value with an analog_get8 reading.  analog_init and bom_init
+ *    must be called for this to work.
+ *
+ *
+ * @param bit_field specifies which elements in bom_val[] should be updated. Use BOM_ALL to refresh all values.
+ *    Ex. if 0x0003 is passed, bom_val[0] and bom_val[1] will be updated.
+ *
+ * @see bom_get
+ **/
+void bom_refresh(int bit_field) {
+    int i;
+    
+    analog_stop_loop();
+    
+    for(i = 0; i < NUM_BOM_LEDS; i++) {
+        if(bit_field & 0x1) {
+            bom_select(i);
+            bom_val[i] = analog_get8(analog_pin);
+        }
+        bit_field = bit_field >> 1;
+    }
+    
+    analog_start_loop();
+}
+
+/**
+ * Gets the bom reading from bom_val[which].  Call bom_refresh beforehand to read new bom values.
+ *
+ * @param which which bom value to return
+ *
+ * @return the bom value
+ *
+ * see bom_refresh
+ **/
+int bom_get(int which) {
+    return bom_val[which];
+}
+
+/** 
+ * Compares all the values in bom_val[] and returns the index to the lowest (max) value element.
+ *
+ * @return index to the lowest (max) bom value element.  -1 if no value is lower than
+ *    BOM_VALUE_THRESHOLD
+ **/
+int bom_get_max(void) {
+    int i, lowest_val, lowest_i;
+    lowest_i = -1;
+    lowest_val = 255;
+    for(i = 0; i < NUM_BOM_LEDS; i++) {
+        if(bom_val[i] < lowest_val) {
+            lowest_val = bom_val[i];
+            lowest_i = i;
+        }
+    }
+    
+    if(lowest_val < BOM_VALUE_THRESHOLD)
+        return lowest_i;
+    else
+        return -1;
+}
+
+/**
+ * Iterates through each bit in the bit_field. If the bit is set, the corresponding emitter will
+ *    be enabled to turn on when bom_on() is called.
+ *    bom_init must be called for this to work. Does nothing if a BOM1.0 is installed
+ *
+ * @param bit_field specifies which leds should be turned on when bom_on is called.  Use BOM_ALL to turn on all bom leds.
+ *    Ex. if 0x0005 is passed, leds 0 and 2 will be turned on.
+ **/
+void bom_set_leds(int bit_field) {
+    int i;
+	unsigned int mask = 1<<(NUM_BOM_LEDS-1);
+	switch(bom_type) {
+    case BOM10:
+        //TODO: put an assert here to alert the user that this should not be called
+        break;
+		
+    case BOM15:
+	    for(i=NUM_BOM_LEDS; i>0; i--)
+	    {
+		    //set the current bit, sending MSB first
+		    digital_output(BOM_DATA, bit_field&mask);
+		    //then pulse the clock
+		    digital_output(BOM_CLOCK, 1);
+		    digital_output(BOM_CLOCK, 0);
+			mask = mask>>1;
+	    }
+        break;
+		
+    case RBOM:
+        //add rbom code here
+        break;
+    }
+}
+
+
+/**
+ * (DEPRECATED) Returns the direction of the maximum BOM reading,
  * as an integer in the range 0-15. 0 indicates to the
  * robot's right, while the rest of the sensors are
  * numbered counterclockwise. This is useful for determining
@@ -74,84 +266,74 @@
  * @see analog_init
  **/
 int get_max_bom(void) {
-	int max_bom_temp = 0;
-	int a, i, j, h;
-    h = 255;
-
-    for (j = 0; j < 16; j++)
-    {
-      i = lookup[j];
-
-      if (i&8)
-        output_high(MONK3);
-      else
-        output_low(MONK3);
-
-      if (i&4)
-        output_high(MONK2);
-      else
-        output_low(MONK2);
-
-      if (i&2)
-        output_high(MONK1);
-      else
-        output_low(MONK1);
-
-      if (i&1)
-        output_high(MONK0);
-      else
-        output_low(MONK0);
-
-      a = analog8(MONKI);
-              
-      if (a < h)
-      {
-        h = a;
-        max_bom_temp = j;
-      }
-
-    }
-
-	//threshold on the bom analog value.
-	//defined in bom.h
-	// if the analog value read is above the threshold, we cannot see a robot
-	// (remember, low means higher intensity).
-	if(h < BOM_VALUE_THRESHOLD) {
-		return max_bom_temp;
-	}
-	else
-		return -1;
+    bom_refresh(BOM_ALL);
+    return bom_get_max();
 }
 
 /**
- * Flashes the BOM. analog_init must be called before this
- * function can be used.
+ * Flashes the BOM.  If using a BOM1.5, only the emitters that have been enabled using
+ * bom_set_leds will turn on.
  * 
- * @see bom_off, analog_init
+ * @see bom_off, bom_set_leds
  **/
 void bom_on(void)
 {
-  output_high(MONKL);
+  switch(bom_type) {
+  case BOM10:
+	digital_output(MONKL, 1);
+	break;
+  case BOM15:
+	digital_output(BOM_STROBE, 1);
+	break;
+  case RBOM:
+	break;
+  }
 }
 
 /**
- * Stops flashing the BOM. analog_init must be called
- * before this function can be used.
+ * Turns off all bom leds.
  * 
- * @see bom_on, analog_init
+ * @see bom_on
  **/
 void bom_off(void)
 {
-  output_low(MONKL);
+  switch(bom_type) {
+  case BOM10:
+	digital_output(MONKL, 0);
+	break;
+  case BOM15:
+	digital_output(BOM_STROBE, 0);
+	break;
+  case RBOM:
+	break;
+  }
 }
 
 /** @} **/ //end group
 
-void output_high(int which) {
-	digital_output(which, 1);
-}
+//select a detector to read
+static void bom_select(char which) {
+	if(bom_type == BOM10)
+	  which = lookup[(int)which];
+	
+    if (which&8)
+      digital_output(select_pins[3], 1);
+    else
+      digital_output(select_pins[3], 0);
 
-void output_low(int which) {
-	digital_output(which, 0);
+    if (which&4)
+      digital_output(select_pins[2], 1);
+    else
+      digital_output(select_pins[2], 0);
+
+    if (which&2)
+      digital_output(select_pins[1], 1);
+    else
+      digital_output(select_pins[1], 0);
+
+    if (which&1)
+      digital_output(select_pins[0], 1);
+    else
+      digital_output(select_pins[0], 0);
+	
 }
-
Index: branches/encoders/code/projects/libdragonfly/battery.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/battery.h	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/battery.h	(revision 1345)
@@ -1,4 +1,30 @@
 /**
+ * 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 battery.h
  * @brief Definitions for checking battery voltage.
  * 
@@ -29,7 +55,7 @@
 int battery(void);
 /** @brief Check if the battery is low. **/
 char battery_low(void);
-/** @brief get an average battery voltage reading. **/
+/** @brief Get an average battery voltage reading. **/
 int battery8_avg(int n_samples);
 
 /** @} **/ //end addtogroup
Index: branches/encoders/code/projects/libdragonfly/dio.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/dio.h	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/dio.h	(revision 1345)
@@ -1,4 +1,30 @@
 /**
+ * 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 dio.h
  * @brief Definitions for digital input / output
  *
@@ -13,17 +39,17 @@
 #define _DIO_H
 
 /*
-these are ALL the pins
-don't use unless you know what you're doing
+  these are ALL the pins
+  don't use unless you know what you're doing
 */
 
 /*
-DIO pins on new dragonfly boards are shown below:
------------------------------------------------
-| _PIN_E6 | _PIN_E7 | _PIN_D2 | _PIN_D3 | VCC |
------------------------------------------------
-| _PIN_E2 | _PIN_E3 | _PIN_E4 | _PIN_E5 | GND |
------------------------------------------------
+  DIO pins on new dragonfly boards are shown below:
+  -----------------------------------------------
+  | _PIN_E6 | _PIN_E7 | _PIN_D2 | _PIN_D3 | VCC |
+  -----------------------------------------------
+  | _PIN_E2 | _PIN_E3 | _PIN_E4 | _PIN_E5 | GND |
+  -----------------------------------------------
 */
 
 /** @brief Port A **/
@@ -168,8 +194,8 @@
 //#define _PIN_G7 63
 
 /*
-These are the header pins (the ones you can connect things to)
-Feel free to use these
+  These are the header pins (the ones you can connect things to)
+  Feel free to use these
 */
 
 /**
Index: branches/encoders/code/projects/libdragonfly/atomic.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/atomic.h	(nonexistent)
+++ branches/encoders/code/projects/libdragonfly/atomic.h	(revision 1345)
@@ -0,0 +1,309 @@
+/* This file is in the dragonfly library directory because it seems to be
+   missing from the AVR libc distribution */
+   
+/* Copyright (c) 2007 Dean Camera
+   All rights reserved.
+
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are met:
+
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in
+     the documentation and/or other materials provided with the
+     distribution.
+
+   * Neither the name of the copyright holders nor the names of
+     contributors may be used to endorse or promote products derived
+     from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+  POSSIBILITY OF SUCH DAMAGE.
+*/
+
+/* $Id: atomic.h,v 1.3 2007/12/20 14:17:56 joerg_wunsch Exp $ */
+
+#ifndef _UTIL_ATOMIC_H_
+#define _UTIL_ATOMIC_H_ 1
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+
+#if !defined(__DOXYGEN__)
+/* Internal helper functions. */
+static __inline__ uint8_t __iSeiRetVal(void)
+{
+    sei();
+    return 1;
+}
+
+static __inline__ uint8_t __iCliRetVal(void)
+{
+    cli();
+    return 1;
+}
+
+static __inline__ void __iSeiParam(const uint8_t *__s)
+{
+    sei();
+    __asm__ volatile ("" ::: "memory");
+    (void)__s;
+}
+
+static __inline__ void __iCliParam(const uint8_t *__s)
+{
+    cli();
+    __asm__ volatile ("" ::: "memory");
+    (void)__s;
+}
+
+static __inline__ void __iRestore(const  uint8_t *__s)
+{
+    SREG = *__s;
+    __asm__ volatile ("" ::: "memory");
+}
+#endif	/* !__DOXYGEN__ */
+
+/** \file */
+/** \defgroup util_atomic <util/atomic.h> Atomically and Non-Atomically Executed Code Blocks
+
+    \code
+    #include <util/atomic.h>
+    \endcode
+
+    \note The macros in this header file require the ISO/IEC 9899:1999
+    ("ISO C99") feature of for loop variables that are declared inside
+    the for loop itself.  For that reason, this header file can only
+    be used if the standard level of the compiler (option --std=) is
+    set to either \c c99 or \c gnu99.
+
+    The macros in this header file deal with code blocks that are
+    guaranteed to be excuted Atomically or Non-Atmomically.  The term
+    "Atomic" in this context refers to the unability of the respective
+    code to be interrupted.
+
+    These macros operate via automatic manipulation of the Global
+    Interrupt Status (I) bit of the SREG register. Exit paths from
+    both block types are all managed automatically without the need
+    for special considerations, i. e. the interrupt status will be
+    restored to the same value it has been when entering the
+    respective block.
+
+    A typical example that requires atomic access is a 16 (or more)
+    bit variable that is shared between the main execution path and an
+    ISR.  While declaring such a variable as volatile ensures that the
+    compiler will not optimize accesses to it away, it does not
+    guarantee atomic access to it.  Assuming the following example:
+
+    \code
+#include <inttypes.h>
+#include <avr/interrupt.h>
+#include <avr/io.h>
+
+volatile uint16_t ctr;
+
+ISR(TIMER1_OVF_vect)
+{
+  ctr--;
+}
+
+...
+int
+main(void)
+{
+   ...
+   ctr = 0x200;
+   start_timer();
+   while (ctr != 0)
+     // wait
+       ;
+   ...
+}
+    \endcode
+
+    There is a chance where the main context will exit its wait loop
+    when the variable \c ctr just reached the value 0xFF.  This happens
+    because the compiler cannot natively access a 16-bit variable
+    atomically in an 8-bit CPU.  So the variable is for example at
+    0x100, the compiler then tests the low byte for 0, which succeeds.
+    It then proceeds to test the high byte, but that moment the ISR
+    triggers, and the main context is interrupted.  The ISR will
+    decrement the variable from 0x100 to 0xFF, and the main context
+    proceeds.  It now tests the high byte of the variable which is
+    (now) also 0, so it concludes the variable has reached 0, and
+    terminates the loop.
+
+    Using the macros from this header file, the above code can be
+    rewritten like:
+
+    \code
+#include <inttypes.h>
+#include <avr/interrupt.h>
+#include <avr/io.h>
+#include <util/atomic.h>
+
+volatile uint16_t ctr;
+
+ISR(TIMER1_OVF_vect)
+{
+  ctr--;
+}
+
+...
+int
+main(void)
+{
+   ...
+   ctr = 0x200;
+   start_timer();
+   sei();
+   uint16_t ctr_copy;
+   do
+   {
+     ATOMIC_BLOCK(ATOMIC_FORCEON)
+     {
+       ctr_copy = ctr;
+     }
+   }
+   while (ctr_copy != 0);
+   ...
+}
+    \endcode
+
+    This will install the appropriate interrupt protection before
+    accessing variable \c ctr, so it is guaranteed to be consistently
+    tested.  If the global interrupt state were uncertain before
+    entering the ATOMIC_BLOCK, it should be executed with the
+    parameter ATOMIC_RESTORESTATE rather than ATOMIC_FORCEON.
+
+*/
+
+/** \def ATOMIC_BLOCK(type)
+    \ingroup util_atomic
+
+    Creates a block of code that is guaranteed to be executed
+    atomically. Upon entering the block the Global Interrupt Status
+    flag in SREG is disabled, and re-enabled upon exiting the block
+    from any exit path.
+
+    Two possible macro parameters are permitted, ATOMIC_RESTORESTATE
+    and ATOMIC_FORCEON.
+*/
+#if defined(__DOXYGEN__)
+#define ATOMIC_BLOCK(type)
+#else
+#define ATOMIC_BLOCK(type) for ( type, __ToDo = __iCliRetVal(); \
+	                       __ToDo ; __ToDo = 0 )
+#endif	/* __DOXYGEN__ */
+
+/** \def NONATOMIC_BLOCK(type)
+    \ingroup util_atomic
+
+    Creates a block of code that is executed non-atomically. Upon
+    entering the block the Global Interrupt Status flag in SREG is
+    enabled, and disabled upon exiting the block from any exit
+    path. This is useful when nested inside ATOMIC_BLOCK sections,
+    allowing for non-atomic execution of small blocks of code while
+    maintaining the atomic access of the other sections of the parent
+    ATOMIC_BLOCK.
+
+    Two possible macro parameters are permitted,
+    NONATOMIC_RESTORESTATE and NONATOMIC_FORCEOFF.
+*/
+#if defined(__DOXYGEN__)
+#define NONATOMIC_BLOCK(type)
+#else
+#define NONATOMIC_BLOCK(type) for ( type, __ToDo = __iSeiRetVal(); \
+	                          __ToDo ;  __ToDo = 0 )
+#endif	/* __DOXYGEN__ */
+
+/** \def ATOMIC_RESTORESTATE
+    \ingroup util_atomic
+
+    This is a possible parameter for ATOMIC_BLOCK. When used, it will
+    cause the ATOMIC_BLOCK to restore the previous state of the SREG
+    register, saved before the Global Interrupt Status flag bit was
+    disabled. The net effect of this is to make the ATOMIC_BLOCK's
+    contents guaranteed atomic, without changing the state of the
+    Global Interrupt Status flag when execution of the block
+    completes.
+*/
+#if defined(__DOXYGEN__)
+#define ATOMIC_RESTORESTATE
+#else
+#define ATOMIC_RESTORESTATE uint8_t sreg_save \
+	__attribute__((__cleanup__(__iRestore))) = SREG
+#endif	/* __DOXYGEN__ */
+
+/** \def ATOMIC_FORCEON
+    \ingroup util_atomic
+
+    This is a possible parameter for ATOMIC_BLOCK. When used, it will
+    cause the ATOMIC_BLOCK to force the state of the SREG register on
+    exit, enabling the Global Interrupt Status flag bit. This saves on
+    flash space as the previous value of the SREG register does not
+    need to be saved at the start of the block.
+
+    Care should be taken that ATOMIC_FORCEON is only used when it is
+    known that interrupts are enabled before the block's execution or
+    when the side effects of enabling global interrupts at the block's
+    completion are known and understood.
+*/
+#if defined(__DOXYGEN__)
+#define ATOMIC_FORCEON
+#else
+#define ATOMIC_FORCEON uint8_t sreg_save \
+	__attribute__((__cleanup__(__iSeiParam))) = 0
+#endif	/* __DOXYGEN__ */
+
+/** \def NONATOMIC_RESTORESTATE
+    \ingroup util_atomic
+
+    This is a possible parameter for NONATOMIC_BLOCK. When used, it
+    will cause the NONATOMIC_BLOCK to restore the previous state of
+    the SREG register, saved before the Global Interrupt Status flag
+    bit was enabled. The net effect of this is to make the
+    NONATOMIC_BLOCK's contents guaranteed non-atomic, without changing
+    the state of the Global Interrupt Status flag when execution of
+    the block completes.
+*/
+#if defined(__DOXYGEN__)
+#define NONATOMIC_RESTORESTATE
+#else
+#define NONATOMIC_RESTORESTATE uint8_t sreg_save \
+	__attribute__((__cleanup__(__iRestore))) = SREG
+#endif	/* __DOXYGEN__ */
+
+/** \def NONATOMIC_FORCEOFF
+    \ingroup util_atomic
+
+    This is a possible parameter for NONATOMIC_BLOCK. When used, it
+    will cause the NONATOMIC_BLOCK to force the state of the SREG
+    register on exit, disabling the Global Interrupt Status flag
+    bit. This saves on flash space as the previous value of the SREG
+    register does not need to be saved at the start of the block.
+
+    Care should be taken that NONATOMIC_FORCEOFF is only used when it
+    is known that interrupts are disabled before the block's execution
+    or when the side effects of disabling global interrupts at the
+    block's completion are known and understood.
+*/
+#if defined(__DOXYGEN__)
+#define NONATOMIC_FORCEOFF
+#else
+#define NONATOMIC_FORCEOFF uint8_t sreg_save \
+	__attribute__((__cleanup__(__iCliParam))) = 0
+#endif	/* __DOXYGEN__ */
+
+#endif
Index: branches/encoders/code/projects/libdragonfly/reset.c
===================================================================
--- branches/encoders/code/projects/libdragonfly/reset.c	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/reset.c	(revision 1345)
@@ -1,13 +1,37 @@
-/*
-reset.h - contains reset() function that resets the AVR
-author: James Kong
+/**
+ * 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.
+ **/
 
-Last Modified:
 
-3.7.2007 - James
-    created reset.c
-*/
-
+/**
+ * @file reset
+ * @brief Reset the Robot
+ *
+ * Implementation of a function to reset the robot.
+ *
+ * @author Colony Project, CMU Robotics Club
+ **/
 #include <avr/interrupt.h>
 #include <util/delay.h>
 #include <reset.h>
@@ -27,9 +51,10 @@
  * and Status Register (MCUCSR).
  **/
 void reset(void) {
-    WDTCR &= 0xF8;
-    WDTCR |= 0x08;
-    _delay_ms(15);
+  WDTCR &= 0xF8;
+  WDTCR |= 0x08;
+  _delay_ms(15);
 }
 
 /**@}**/ //end defgroup
+
Index: branches/encoders/code/projects/libdragonfly/bom.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/bom.h	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/bom.h	(revision 1345)
@@ -1,4 +1,30 @@
 /**
+ * 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 bom.h
  * @brief Definitions for using the BOM
  * 
@@ -16,12 +42,42 @@
  * @addtogroup bom
  * @{
  **/
+ 
+/** @brief Include all elements in the 16-bit bitfield **/
+#define BOM_ALL 0xFFFF
 
-/** @brief Returns the location of the maximum BOM reading. **/
-int get_max_bom( void );
-/** @brief Turns the BOM on. **/
+/** @brief Original BOM - No Range, No Individual LED control **/
+#define BOM10     0
+
+/** @brief BOM 1.5 - No Range, Individual LED control **/
+#define BOM15   1
+
+/** @brief RBOM - Range, Individual LED control **/
+#define RBOM    2
+
+
+/** @brief Initialize the bom according to bom type **/
+void bom_init(char type);
+
+/** @brief Refresh bom_val[] with new values from analog8.  analog_init and bom_init must be called for this to work. **/
+void bom_refresh(int bit_field);
+
+/** @brief Gets the bom reading from bom_val[which].  Call bom_refresh beforehand to read new bom values. **/
+int bom_get(int which);
+
+/** @brief Compares all the values in bom_val[] and returns the index to the highest value element. **/
+int bom_get_max(void);
+
+/** @brief Enables the selected bom leds on a BOM1.5 **/
+void bom_set_leds(int bit_field);
+
+/** @brief (DEPRECATED) Gets and compares all bom values.  Returns the index to the highest value element. **/
+int get_max_bom(void);
+
+/** @brief Turns on all BOM leds, or turns on enabled leds on a BOM1.5. **/
 void bom_on(void);
-/** @brief Turns the BOM off. **/
+
+/** @brief Turns off all bom leds. **/
 void bom_off(void);
 
 /** @} **/
Index: branches/encoders/code/projects/libdragonfly/rangefinder.c
===================================================================
--- branches/encoders/code/projects/libdragonfly/rangefinder.c	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/rangefinder.c	(revision 1345)
@@ -1,25 +1,60 @@
+/**
+ * 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 rangefinder.c
+ * @brief Rangefinders
+ *
+ * Implementation of functions for rangefinder use.
+ *
+ * @author Colony Project, CMU Robotics Club
+ **/
+
 /*
-Authors: James Kong and Greg Tress
+  Authors: James Kong and Greg Tress
 
-Last Modified: 4/30/06 by James
--Started log_distance conversion function !!!NOT COMPLETE!!!
--Cleaning up comments
+  Last Modified: 4/30/06 by James
+  -Started log_distance conversion function !!!NOT COMPLETE!!!
+  -Cleaning up comments
 
------------------
-rangefinder.c
-Using Sharp GP2D02 IR Rangefinder
+  -----------------
+  rangefinder.c
+  Using Sharp GP2D02 IR Rangefinder
 
-Vin is the input to the rangefinder, designated RANGE_CTRL.
-Vout is the output from the rangefinder, designated RANGE_IN# where # is the rangefinder you are reading from
+  Vin is the input to the rangefinder, designated RANGE_CTRL.
+  Vout is the output from the rangefinder, designated RANGE_IN# where # is the rangefinder you are reading from
 
-Expected Initial Conditions:
-Vin is high and Vout should read high.
+  Expected Initial Conditions:
+  Vin is high and Vout should read high.
 
-Usage:
-1.) Set Vin low. Vout should read low.
-2.) Wait for high on Vout.
-3.) Begin clocking Vin and reading 8 bits from Vout (MSB first).
-4.) Set Vin high for 2ms or more to turn off rangefinder
+  Usage:
+  1.) Set Vin low. Vout should read low.
+  2.) Wait for high on Vout.
+  3.) Begin clocking Vin and reading 8 bits from Vout (MSB first).
+  4.) Set Vin high for 2ms or more to turn off rangefinder
 
 */
 
@@ -27,28 +62,27 @@
 #include "rangefinder.h"
 
 /*
-read_distance returns the 8-bit reading from the rangefinder
-parameters:
-range_id - dio pin set as the rangefinder Vout [i.e. RANGE_IN0]
+  read_distance returns the 8-bit reading from the rangefinder
+  parameters:
+  range_id - dio pin set as the rangefinder Vout [i.e. RANGE_IN0]
 
-NOTE:
-The Sharp GD2D02 returns values on a decreasing logrithmic scale.
-So higher values correspond to closer distances.  Use linearize_distance to convert to normal centimeter scale.
-Also, when reading distances closer than 8cm, the Sharp GD2D02 will return lower values than the values at 8cm.
-At this point, we are only reading from one rangefinder [RANGE_IN0].
+  NOTE:
+  The Sharp GD2D02 returns values on a decreasing logrithmic scale.
+  So higher values correspond to closer distances.  Use linearize_distance to convert to normal centimeter scale.  Also, when reading distances closer than 8cm, the Sharp GD2D02 will return lower values than the values at 8cm.
+  At this point, we are only reading from one rangefinder [RANGE_IN0].
 */
 
 // constants
 /* Nasty IR approximation table
-  I'm using this for the heck of it.  We can do whatever.
+   I'm using this for the heck of it.  We can do whatever.
 
-  Note the minimum value is .4V (20), and the maximum is 2.6V (133).
-  Gives distance in mm.
+   Note the minimum value is .4V (20), and the maximum is 2.6V (133).
+   Gives distance in mm.
 
-  excel formula(valid for inputs 20-133):  ROUND(2353.6*(E2^(-1.1146))*10,0)
+   excel formula(valid for inputs 20-133):  ROUND(2353.6*(E2^(-1.1146))*10,0)
 
-  This is only valid for the GP2D12, with objects directly ahead and more than
-  10cm from the detector.  See the datasheet for more information.
+   This is only valid for the GP2D12, with objects directly ahead and more than
+   10cm from the detector.  See the datasheet for more information.
 */
 
 #define MIN_IR_ADC8 20
@@ -55,14 +89,14 @@
 #define MAX_IR_ADC8 133
 
 static int IR_dist_conversion[114] = {
-800,791,751,714,681,651,623,597,574,552,531,512,494,478,462,447
-,434,421,408,397,386,375,365,356,347,338,330,322,315,307,301,294
-,288,282,276,270,265,260,255,250,245,241,237,232,228,224,221,217
-,213,210,207,203,200,197,194,191,189,186,183,181,178,176,173,171
-,169,166,164,162,160,158,156,154,152,151,149,147,145,144,142,140
-,139,137,136,134,133,131,130,129,127,126,125,124,122,121,120,119
-,118,117,115,114,113,112,111,110,109,108,107,106,105,105,104,103
-,102,101
+  800,791,751,714,681,651,623,597,574,552,531,512,494,478,462,447
+  ,434,421,408,397,386,375,365,356,347,338,330,322,315,307,301,294
+  ,288,282,276,270,265,260,255,250,245,241,237,232,228,224,221,217
+  ,213,210,207,203,200,197,194,191,189,186,183,181,178,176,173,171
+  ,169,166,164,162,160,158,156,154,152,151,149,147,145,144,142,140
+  ,139,137,136,134,133,131,130,129,127,126,125,124,122,121,120,119
+  ,118,117,115,114,113,112,111,110,109,108,107,106,105,105,104,103
+  ,102,101
 };
 
 int linearize_distance(int value);
@@ -82,7 +116,8 @@
  *
  * @see range_read_distance
  **/
-void range_init(void){
+void range_init(void)
+{
   digital_output(_PIN_B4,0);
 }
 
@@ -97,27 +132,18 @@
  *
  * @see range_init
  **/
-int range_read_distance (int range_id) {  
+int range_read_distance (int range_id) {
   return linearize_distance(analog8(range_id));
 }
 
 /** @} **/ //end defgroup
 
-int linearize_distance(int value)
-{
-  if(value < MIN_IR_ADC8)
-  {
+int linearize_distance(int value) {
+  if(value < MIN_IR_ADC8) {
     return -1;
-  }
-  else if(value > MAX_IR_ADC8)
-  {
+  } else if(value > MAX_IR_ADC8) {
     return -1;
-  }
-  else
-  {
+  } else {
     return IR_dist_conversion[value - MIN_IR_ADC8];
   }
 }
-
-
-
Index: branches/encoders/code/projects/libdragonfly/reset.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/reset.h	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/reset.h	(revision 1345)
@@ -1,10 +1,36 @@
 /**
+ * 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 reset.h
  * @brief Contains function(s) for resetting the robots
  * 
  * @author Colony Project, CMU Robotics Club, James Kong
-*/
+ */
 
 #ifndef _RESET_H_
 #define _RESET_H_
Index: branches/encoders/code/projects/libdragonfly/rangefinder.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/rangefinder.h	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/rangefinder.h	(revision 1345)
@@ -1,4 +1,30 @@
 /**
+ * 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 rangefinder.h
  * @brief Contains rangefinder declarations and functions
  * 
Index: branches/encoders/code/projects/libdragonfly/lights.c
===================================================================
--- branches/encoders/code/projects/libdragonfly/lights.c	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/lights.c	(revision 1345)
@@ -1,258 +1,521 @@
+/**
+ * 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 ligths.c
+ * @brief Orbs
+ *
+ * Implemenation for the orbs (tri-colored LEDs)
+ *
+ * @author Colony Project, CMU Robotics Club
+ * @bug Unfinished
+ **/
+
 /*
 lights.c
-Controls orb1 and orb2. Also contains the framework for a software PWM that may be used for servos in the future.
+Controls orb1 and orb2. Can be extended for a software PWM that may be used for servos in the future (although maybe
+using a different timer might be preferable).
 
 author: CMU Robotics Club, Colony Project
 
 Change Log:
-2.4.07 - Aaron
-	Revamped orb code so it works.  Need to check interaction with rtc, and tweak some colors.
+3/31/2009 - Martin
+    Rewritten from scratch. Fixes code duplication, long ISRs, bugs, unnecessary synchronized code, memory waste
+*/
 
-2.1.07 - James
-	Modified sort_buffer() to prune for repeats.  PWM now uses orb_buf_size for the number of orb values in orb_time_arr[].
-		Changed sorting algorithm used in sort_buffer() to selection sort (faster). And it works now.
 
-1.25.07 - KWoo
-	Deleted old FF+ code to make it cleaner. Commented code. This all works. Note however that if you ever plan to use the
-		software PWM (which is this) you will need to change the implementation of orb_enable() and orb_disable() to not
-		shutdown the PWM.
 
-*/
+/*
+ * Test cases:
+ *   - The following code has to work without flickering:
+ *     orb_init_pwm(); while(1) { orbs_set(1,1,1,254,254,254); }
+ */
 
+/*
+ * Possible optimizations:
+ *   - Use pointers instead of indicies for current_pwm_channel
+ *   - Optimize output_compare()
+ *   - Use a different sorting algorithm (see sort_orbs_buffer for further comments on this issue)
+ *   - Use pointers in fill_orbs_buffer
+ *   - Optimized orb_set (use the knowledge that there are only 3 distinct values, don't use a loop but unroll the
+ *     sorting, which is no problem for 3 values)
+ *   - Use a lower update frequency. The next higher prescaler value leads to a frequency of 30Hz which is too low (the
+ *     orbs are flickering). So the timer would have to be reloaded manually after 127 to generate 60Hz. This would
+ *     decrease the resolution from 8 to 7 bit, but 128 steps should still be enough.
+ *   - On setting the orbs, combine channels with the same time. This would reduce the all-values-equal OC interrupt
+ *     (30us) to the time of one regular OC interrupt (6us/10us). Also, it would reduce the total cpu usage whenever
+ *     some of the values are equal.
+ *
+ * When code is changed, the performance measurements above should be redone.
+ */
+
+
+
 #include "lights.h"
-#include "dragonfly_lib.h"
+
 #include <avr/interrupt.h>
 
+#include "dragonfly_lib.h"
 
-#define ORB_RESET 1025
 
+// ***************
+// ** Constants **
+// ***************
+
+#define NUM_ORBS 2   // Number or orbs
+#define NUM_COLORS 3 // Number of colors per orb
+#define num_pwm_channels NUM_ORBS*NUM_COLORS
+
+
+// *********
+// ** I/O **
+// *********
+
+// Orb port
 #define ORBPORT PORTC
+#define ORBDDR  DDRC
 
-#define ORBDDR DDRC
+// Orb pins
+#define ORB1_RED   0
+#define ORB1_GREEN 1
+#define ORB1_BLUE  2
+#define ORB2_RED   4
+#define ORB2_GREEN 5
+#define ORB2_BLUE  6
 
-#define ORBMASK 0x77
 
+// ***************
+// ** Debugging **
+// ***************
 
-/***** Port and Pin Definitions ****/
+//#define LIGHTS_DEBUG
+#undef LIGHTS_DEBUG
 
-//Orb Ports and Registers
-#define ORB_PORT        PORTC
-#define ORB_DDR         DDRC
+#define LIGHTS_DEBUG_INIT                             DDRF=6;
+#define LIGHTS_DEBUG_OVERFLOW_INTERRUPT_START         PORTF|=4;
+#define LIGHTS_DEBUG_OVERFLOW_INTERRUPT_END           PORTF&=~4;
+#define LIGHTS_DEBUG_OUTPUT_COMPARE_INTERRUPT_START   PORTF|=2;
+#define LIGHTS_DEBUG_OUTPUT_COMPARE_INTERRUPT_END     PORTF&=~2;
+#define LIGHTS_DEBUG_APPLY_START                      //PORTF|=2;
+#define LIGHTS_DEBUG_APPLY_END                        //PORTF&=~2;
 
-//Orb Pins
-#define ORB1_RED        0x00
-#define ORB1_GREEN      0x01
-#define ORB1_BLUE       0x02
-#define ORB2_RED        0x04
-#define ORB2_GREEN      0x05
-#define ORB2_BLUE       0x06
 
+// ***********
+// ** Masks **
+// ***********
 
-#define ORB_COUNT 8	//please dont change this, or bad things might happen
+// Some useful bit masks. All of them are are calculated from the I/O definitions above. The calculations should be done
+// at compile time (even if they are not, they are only executed once at startup).
 
-// an orb node
-struct ORB_NODE {
-   uint8_t num;
-   uint16_t angle;
+// Masks for the individual LEDs
+#define orb1_red_mask    _BV (ORB1_RED  )
+#define orb1_green_mask  _BV (ORB1_GREEN)
+#define orb1_blue_mask   _BV (ORB1_BLUE )
+#define orb2_red_mask    _BV (ORB2_RED  )
+#define orb2_green_mask  _BV (ORB2_GREEN)
+#define orb2_blue_mask   _BV (ORB2_BLUE )
+
+// Mask for all LEDs
+#define all_orbs_mask \
+    orb1_red_mask | orb1_green_mask | orb1_blue_mask | \
+    orb2_red_mask | orb2_green_mask | orb2_blue_mask;
+
+// Mask for the individual LEDs, organized as an array for programmatic access. The layout of this array is
+// orb_mask[orb_num, color_num]
+const uint8_t orb_mask[NUM_ORBS][NUM_COLORS]={
+    { orb1_red_mask, orb1_green_mask, orb1_blue_mask },
+    { orb2_red_mask, orb2_green_mask, orb2_blue_mask }
 };
 
-//the change in an orb
-struct ORB_CHANGE {
-   uint16_t port_val;
-   uint16_t split_time_period;
+// ***********
+// ** Types **
+// ***********
+
+struct pwm_channel_t { // 2 bytes
+    uint8_t time;
+    uint8_t mask;
 };
 
-// the status of an orb
-struct ORB_STATUS_STRUCT {
-   struct ORB_NODE orbs[ORB_COUNT];
-   uint16_t orb_angles[ORB_COUNT];
-   struct ORB_CHANGE changes[ORB_COUNT+1];
-   uint8_t change_count;
-   uint8_t new_angles;
-   uint8_t current_orb;
+struct pwm_t { // 13 bytes
+    uint8_t init_mask;
+    struct pwm_channel_t channel[num_pwm_channels];
+};
 
-} ORB_STATUS;
 
-void orb_sort(void);
-void orb_setup_pulse(void);
+// ***************
+// ** Variables **
+// ***************
 
-SIGNAL (SIG_OUTPUT_COMPARE3C){
+// Whether to use PWM (true) or binary (false) orb mode. Not volatile because it's only read once per function.
+bool enable_orb_pwm=true;
 
-		//pull the correct ones down
-      ORBPORT &= (~ORBMASK)|(ORB_STATUS.changes[ORB_STATUS.current_orb].port_val);
+// The PWM channels and the buffer pointers. This data structure is triple buffered, see above for the reasons. Not
+// volatile because they are not modified asynchronously (the read buffer is never written to asynchronously).
+struct pwm_t pwm_buffer[3];
 
-      ++ORB_STATUS.current_orb; //now look at next orb transition
+// The front buffer the ISR reads from. Other threads may not touch this pointer or the buffer it points to. Not
+// volatile because it may only be modified by the ISR.
+struct pwm_t *pwm_read_buffer =&pwm_buffer[0];
 
-      if (ORB_STATUS.current_orb < ORB_STATUS.change_count) { //if it isnt the end...
-	  
-			//setup timer for next pull down
-         OCR3C = TCNT3+ORB_STATUS.changes[ORB_STATUS.current_orb].split_time_period;
-		 
-     }
-      else { //we are done with these pulses
-		orb_setup_pulse();
-      }
+// The back buffer we can write to. The ISR may not touch this pointer or the buffer it points to. Not volatile because
+// it may only be modified by the caller.
+struct pwm_t *pwm_write_buffer=&pwm_buffer[1]; 
 
-}
+// The middle buffer to flip the write or read buffer with. Not volatile because it is only read once per function.
+struct pwm_t *pwm_free_buffer =&pwm_buffer[2]; 
 
+// Whether to perform a page flip on the beginning of the next PWM cycle. Not volatile because it is only read once
+// per function.
+bool pwm_page_flip=false; // Whether to do a page flip on the next overflow
 
-//sets a channel to a value
-void orb_set_angle(int orb, int angle) {
-	uint8_t mysreg;
+// The orb value array. Orb values are written here to be sorted into pwm_channels. Not volatile because all accesses
+// are from guarded (thread safe) functions.
+uint8_t orb_values[NUM_ORBS][NUM_COLORS];
+
+
+// ****************
+// ** Timer ISRs **
+// ****************
+
+// Not volatile because it is only accessed in the interrupt handler.
+uint8_t current_pwm_channel=0;
+
+
+static void output_compare (void) {
+	// This function is called when an output compare condition may have occured.
 	
-	orb=orb&0x07; //only have 8
-	angle=angle&0xff; //only accept 0-255
-	angle=255-angle; //inverse intensity
-	angle=angle<<2; //scale up so that we dont run it too often
-	angle+=3; //0 values dont really work
-   if (ORB_STATUS.orb_angles[orb] != angle) { //if the angle has changed
-	  mysreg=SREG; 
-	  cli(); //disable interrupts
-      ORB_STATUS.orb_angles[orb] = angle; //update angle
-      ORB_STATUS.new_angles = 1;
-	  SREG=mysreg; //put interrupt status back
-   }
+    // If the OC interrupt is executed without delay, TCNT0==time+1 (where time==OCR0), because the interrupt flag is
+	// queued at the next timer clock cycle after an output compare.
+
+    // What may happen here is that the interrupt is delayed for more than one timer clock cycle (33 us). In that case,
+	// the timer has already counted on and TCNT0 is bigger than current_channel_timer. Also, while during the ISR no
+	// other interrupts will occur, the timer may still count on. Thus, we have to check the following channel as well.
+
+	// Some optimization is possible in this function.
+
+    while (1) {
+		// The timer value at which the output compare interrupt should occur (one timer clock cycle after the output
+		// compare condition is detected).
+		uint8_t current_channel_time=pwm_read_buffer->channel[current_pwm_channel].time+1;
+		
+		// If the counter is not at this time yet, we don't have to do anything right now.
+		if (current_channel_time>TCNT0) return;
+		
+		// We have an output compare condition for the current channel.
+		
+        // Turn the current channel off
+        ORBPORT|=pwm_read_buffer->channel[current_pwm_channel].mask;
+
+		// If this was the last channel, exit
+		if (current_pwm_channel==num_pwm_channels-1) return;
+
+        // Increment the channel index
+        current_pwm_channel++;
+
+        // There is a next channel, load its OCR value
+        if (pwm_read_buffer->channel[current_pwm_channel].time<255)
+            OCR0=pwm_read_buffer->channel[current_pwm_channel].time;
+    }
 }
 
+SIGNAL (SIG_OVERFLOW0) {
+#ifdef LIGHTS_DEBUG
+    LIGHTS_DEBUG_OVERFLOW_INTERRUPT_START
+#endif
 
-void orb_sort(void) {
-   int done = 0, i;
-   
-   while (! done) {
-      done = 1;
+    if (pwm_page_flip) {
+        // Flip the read buffer with the free buffer. We are in an ISR (and we didn't re-enable interrupts), so we don't
+        // have to synchronize explicitly.
+        struct pwm_t *temp = pwm_read_buffer;
+        pwm_read_buffer    = pwm_free_buffer;
+        pwm_free_buffer    = temp;
+        pwm_page_flip=false;
+    }
 
-      for (i = 0; i < ORB_COUNT - 1; ++i) {  //loop through all
-	  
-			//if they are out of order, swap them
-         if (ORB_STATUS.orbs[i].angle > ORB_STATUS.orbs[i+1].angle) {
-            ORB_STATUS.orbs[i].angle ^= ORB_STATUS.orbs[i+1].angle;
-            ORB_STATUS.orbs[i+1].angle ^= ORB_STATUS.orbs[i].angle;
-            ORB_STATUS.orbs[i].angle ^= ORB_STATUS.orbs[i+1].angle;
+    // Turn only the appropriate PWM channels on. Do this directly on the orb port because at this point all orbs should
+    // be off anyway.
+    ORBPORT|=all_orbs_mask;
+    ORBPORT&=pwm_read_buffer->init_mask;
+    
+    // Start at the first channel
+    current_pwm_channel=0;
+    
+    // Load the first OCR
+    OCR0=pwm_read_buffer->channel[current_pwm_channel].time;
 
-            ORB_STATUS.orbs[i].num ^= ORB_STATUS.orbs[i+1].num;
-            ORB_STATUS.orbs[i+1].num ^= ORB_STATUS.orbs[i].num;
-            ORB_STATUS.orbs[i].num ^= ORB_STATUS.orbs[i+1].num;
+	// If this interrupt was delayed, we might already have an output compare condition.
+	output_compare ();
 
-            done = 0;
-         }
-      }
-   }
+#ifdef LIGHTS_DEBUG
+    LIGHTS_DEBUG_OVERFLOW_INTERRUPT_END
+#endif
 }
 
-//calculate the split times
-void orb_setup_pulse(void) {
-   int i;
-   uint16_t my_port;
-   uint16_t sum = 0;
-   uint16_t split_time;
+SIGNAL(SIG_OUTPUT_COMPARE0) {
+#ifdef LIGHTS_DEBUG
+    LIGHTS_DEBUG_OUTPUT_COMPARE_INTERRUPT_START
+#endif
 
-   my_port = 0xff; //all on
+	// We have an output compare condition.
+	output_compare ();
+    
+#ifdef LIGHTS_DEBUG
+    LIGHTS_DEBUG_OUTPUT_COMPARE_INTERRUPT_END
+#endif
+}
 
-   if (ORB_STATUS.new_angles) {
 
-      ORB_STATUS.change_count = 0;
-	  for (i = 0; i < ORB_COUNT; ++i) { //get the new values
-         ORB_STATUS.orbs[i].angle = ORB_STATUS.orb_angles[ORB_STATUS.orbs[i].num];
-      }
 
-      orb_sort(); //sort them
-      ORB_STATUS.new_angles = 0;
+// ************************************
+// ** Internal orb setting functions **
+// ************************************
 
-      for (i = 0; i < ORB_COUNT; ++i) { //calculate split times
-         split_time = ORB_STATUS.orbs[i].angle - sum;
-         my_port &= ~_BV(ORB_STATUS.orbs[i].num);
-		 
-         for (; i < ORB_COUNT - 1 && ORB_STATUS.orbs[i].angle == ORB_STATUS.orbs[i+1].angle; ++i) {
-            my_port &= ~_BV(ORB_STATUS.orbs[i+1].num); //look for doups
-         }
-		 
-         ORB_STATUS.changes[ORB_STATUS.change_count].port_val = my_port; //which pins are low
-         ORB_STATUS.changes[ORB_STATUS.change_count].split_time_period = split_time;
-         
-		 ++ORB_STATUS.change_count;
-		 
-         sum += split_time;
-      }
+static void sort_orbs_buffer (void) {
+    // This function applies a bubble sort to sort the elements of the pwm_write_buffer->channel array by the time
+    // field.
+    // This implementation is heavily optimized. Note that due to the low (and constant) number of elements to be
+    // sorted, the runtime complexity (O(n^2) for bubble sort) is not relevant here. In fact, a more advanced algorithm
+    // like quick sort or merge sort might even be slower due to higher overhead.
+    // That said, it is possible that selection sort (which is also in O(n^2)) would be faster that bubble sort because
+    // it only has to do a maximum of (n-1) swapping steps (as opposed to n*(n-1)/2 for bubble sort). However, the check
+    // if the elements are already in the correct order would either have to be left out (doing the full search every
+    // time, even if the array is already sorted) or done explicitly, so selection sort might actually be slower than
+    // bubble sort, especially if the array is already sorted or almost sorted.
+    
+    // This implementation uses macros to make the algorithm more clear because the loop is rolled out and the function
+    // would become quite long without macros.
+    
+    // Macro to swap two values of any type. Requires a variable of the appropriate type called swap_temp.
+    #define swap(a,b) { swap_temp=a; a=b; b=swap_temp; }
 
-      ORB_STATUS.changes[ORB_STATUS.change_count].port_val = my_port;
-      ORB_STATUS.changes[ORB_STATUS.change_count].split_time_period = ORB_RESET - sum; //get a constant period
+    // Macro to do one bubble sorting step (compare & swap)
+    #define bubble \
+        if(a->time > b->time) \
+        { \
+            swap (a->time, b->time); \
+            swap (a->mask, b->mask); \
+            done=false; \
+        }
+    
+    // Macro to move to the next bubble sort pair
+    #define next { a++; b++; }
 
-      ++ORB_STATUS.change_count;
+    // Whether no change was made during the last run, which means that all values are already in correct order.
+    bool done;
+    
+    // A temporary variable for swapping.
+    uint8_t swap_temp;
+    
+    // Precompute the first PWM channel (tested faster).
+    struct pwm_channel_t *first=&(pwm_write_buffer->channel[0]);
 
-   }
+    // Pointers to the two PWM channels under inspection
+    struct pwm_channel_t *a, *b;
 
+    // The actual sorting
+    a=first; b=a+1; done=true;
+    bubble next bubble next bubble next bubble next bubble
+    if (done) return;
 
+    a=first; b=a+1; done=true;
+    bubble next bubble next bubble next bubble
+    if (done) return;
 
-   ORB_STATUS.current_orb = 0;
+    a=first; b=a+1; done=true;
+    bubble next bubble next bubble
+    if (done) return;
 
-    ORBPORT |= ORBMASK; //start with all high
-	OCR3C = TCNT3 + ORB_STATUS.changes[0].split_time_period; //wait for first split
+    a=first; b=a+1; done=true;
+    bubble next bubble
+    if (done) return;
 
+    a=first; b=a+1; done=true;
+    bubble
+    if (done) return;
+
+    // Undefine the macros so they do not disturb some other function.
+    #undef next
+    #undef bubble
+    #undef swap
 }
 
-/**
- * @defgroup orbs Orbs
- * @brief Functions for controlling the color of the orbs.
- * 
- * Functions for controlling the color and lighting of the orbs.
- *
- * @{
- **/
+static void fill_orbs_buffer (void) {
+    // We do not use a loop here because it introduces 27us overhead, which is quite much, given the total time for
+    // optimized copying and sorting of 34us (elements already in correct order) to 71 us (elements in reverse order).
+    
+    #define copy_value(orb, color) \
+        index=NUM_COLORS*orb+color; \
+        time=orb_values[orb][color]; \
+        mask=orb_mask[orb][color]; \
+        \
+        pwm_write_buffer->channel[index].time=time-1; \
+        pwm_write_buffer->channel[index].mask=mask; \
+        \
+        if (time!=0) \
+            pwm_write_buffer->init_mask &= ~mask; \
 
-/**
- * Initializes the PWM for Orb control. This must be called before 
- * the orbs are used for them to function.
- **/
-void orb_init() 
-{	
-   int i;
-   uint8_t mysreg;
-   
-   ORBDDR |= ORBMASK;	//all outputs
-   
-	mysreg=SREG;
-	cli(); //turn off interrupts for now
+    uint8_t index, time, mask;
+    copy_value(0,0); copy_value(0,1); copy_value(0,2);
+    copy_value(1,0); copy_value(1,1); copy_value(1,2);
+    
+    #undef copy_value
+}
 
-	//init everything
+static void apply_orbs (void) {
+	/*
+	 * Some timing tests: Time for apply_orbs with interrupts disabled, in us:
+     *             Values in:     Correct order      Reverse order
+     * Naive bubble sort:         148                217
+     * Aborting bubble sort:       71                232
+     * Only count to top:          73                189
+     *
+     * Loops rolled out:           61                120
+     * Using pointers:             62                 98
+     * Copy loop also rolled out:  35                 72
+	 *
+	 * Note that rolling out both loops and using pointers saves 52%/62% of time! 27us were spent on loop overhead,
+	 * which is quite much, considering an optimized total time for copying and sorting or 35us.
+	 */
 
-   for (i = 0; i < ORB_COUNT; ++i) {
-      ORB_STATUS.orbs[i].num = i;
-      ORB_STATUS.orbs[i].angle = 1023;	//127 is a pretty stupid start angle, but oh well
-      ORB_STATUS.orb_angles[i] = 1023;
-   }
+#ifdef LIGHTS_DEBUG
+    LIGHTS_DEBUG_APPLY_START
+#endif
 
-   ORB_STATUS.new_angles = 1;
-   ORB_STATUS.change_count = 0;
+    if (enable_orb_pwm) {
+        // PWM mode
+        
+        pwm_write_buffer->init_mask=~0;
+    
+        // 1. Write the orb values and corresponding masks to the pwm channels
+        // array unsorted.
+        fill_orbs_buffer ();
 
-	//init timer3
-	TCCR3A = 0; 
-	TCCR3B = _BV(CS31); //prescale = 8
-	TCCR3C = 0;
-	ETIMSK |= _BV(OCIE3C); //turn on oc3c interrupt
-	OCR3C = TCNT3+ORB_RESET;
+        // 2. sort the buffer.
+        sort_orbs_buffer ();
 
-	SREG=mysreg;
+        // Flip the write buffer with the free buffer.
+        SYNC {
+            struct pwm_t *temp = pwm_write_buffer;
+            pwm_write_buffer   = pwm_free_buffer;
+            pwm_free_buffer    = temp;
+        }
+        
+        // On the next overflow, flip the read buffer with the free buffer.
+        pwm_page_flip=true;
+    }
+    else {
+        // Binary mode.
+        // The outputs are inverted.
+        uint8_t on=0;
+        
+        if (orb_values[0][0]) on |= orb_mask[0][0];
+        if (orb_values[0][1]) on |= orb_mask[0][1];
+        if (orb_values[0][2]) on |= orb_mask[0][2];
+        if (orb_values[1][0]) on |= orb_mask[1][0];
+        if (orb_values[1][1]) on |= orb_mask[1][1];
+        if (orb_values[1][2]) on |= orb_mask[1][2];
+    
+        // Write the new orb states to the output port. Synchronized because it is a RMW operation.
+        SYNC {
+            uint8_t value=ORBPORT;
+            value |= all_orbs_mask; // All orbs off
+            value &= ~on; // Selected orbs on
+            ORBPORT=value;
+        }
+    }
+            
+#ifdef LIGHTS_DEBUG
+    LIGHTS_DEBUG_APPLY_END
+#endif
 }
 
+static void set_orb_values (uint8_t num, uint8_t red, uint8_t green, uint8_t blue) {
+    // Write the values to the array, but do not sort them yet, as we might want to write the other orb values first so
+    // we don't have to sort twice.
+    // Any function calling this function will probably want to call apply_orbs() afterwards.
+    orb_values[num][0]=red;
+    orb_values[num][1]=green;
+    orb_values[num][2]=blue;
+}
+
+
+// ***********************
+// ** RGB color setting **
+// ***********************
+
+// All of these functions use set_orb_values to set the actual values, and then call apply_orbs() to apply the changes.
+// set_orb_values should be used (even though it would be faster to set the array directly) because the binary/pwm mode
+// has to be handled.
+// All of these functions must be 
+
+uint8_t orb_lock=0;
+
 /**
- * Set both orbs to the color specified. orb_init must
- * be called before this function may be used.
+ * Sets the specified orb to the specified color. The orbs must be initialized before this function may be used.
+ * Note that, when setting both orbs, using orbs_set is faster then setting the orbs individually because the values are
+ * only sorted once.
  *
- * @param red_led the red component of the color
- * @param green_led the green component of the color
- * @param blue_led the blue component of the color
+ * @param num the number of the orb to set (0 or 1)
+ * @param red the red value for the specified orb
+ * @param green the green value for the specified orb
+ * @param blue the blue value for the specified orb
+ * @see 
+ */
+void orb_n_set (uint8_t num, uint8_t red, uint8_t green, uint8_t blue) {
+	REQUIRE_LOCK_OR_RETURN(orb_lock);
+	
+    set_orb_values (num, red, green, blue);
+    apply_orbs ();
+	
+	RELEASE_LOCK(orb_lock);
+}
+
+/**
+ * Set orb1 to the color specified. The orbs must be initialized before this function may be used. Note that, when
+ * setting both orbs, using orbs_set is faster then setting the orbs individually because the values are only sorted
+ * once.
  *
+ * @param red the red component of the color
+ * @param green the green component of the color
+ * @param blue the blue component of the color
+ *
  * @see orb_init
  **/
-void orb_set(unsigned char red_led, unsigned char green_led, unsigned char blue_led) {
-	orb1_set(red_led,green_led,blue_led);
-	orb2_set(red_led,green_led,blue_led);
+void orb1_set (uint8_t red, uint8_t green, uint8_t blue) {
+	REQUIRE_LOCK_OR_RETURN(orb_lock);
+	
+    set_orb_values (0, red, green, blue);
+    apply_orbs ();
 
+	RELEASE_LOCK(orb_lock);
 }
 
 /**
- * Set orb1 to the color specified. orb_init must
- * be called before this function may be used.
+ * Set orb2 to the color specified. The orbs must be initialized before this function may be used. Note that, when
+ * setting both orbs, using orbs_set is faster then setting the orbs individually because the values are only sorted
+ * once.
  *
  * @param red_led the red component of the color
  * @param green_led the green component of the color
@@ -260,124 +523,235 @@
  *
  * @see orb_init
  **/
-void orb1_set(unsigned char red_led, unsigned char green_led, unsigned char blue_led) {
-	orb_set_angle(0,red_led);
-	orb_set_angle(1,green_led);
-	orb_set_angle(2,blue_led);
+void orb2_set (uint8_t red, uint8_t green, uint8_t blue) {
+	REQUIRE_LOCK_OR_RETURN(orb_lock);
+	
+    set_orb_values (1, red, green, blue);
+    apply_orbs ();
+
+	RELEASE_LOCK(orb_lock);
 }
 
 /**
- * Set orb2 to the color specified. orb_init must
- * be called before this function may be used.
+ * Set both orbs to the color specified. The orbs must be initialized before this function may be used.
  *
  * @param red_led the red component of the color
  * @param green_led the green component of the color
  * @param blue_led the blue component of the color
  *
- * @see orb_init
+ * @see orb_init, orb1_set, orb2_set
  **/
-void orb2_set(unsigned char red_led, unsigned char green_led, unsigned char blue_led) {	
-	orb_set_angle(4,red_led);
-	orb_set_angle(5,green_led);
-	orb_set_angle(6,blue_led);
+void orb_set (uint8_t red, uint8_t green, uint8_t blue) {
+	REQUIRE_LOCK_OR_RETURN(orb_lock);
+	
+    set_orb_values (0, red, green, blue);
+    set_orb_values (1, red, green, blue);
+    apply_orbs ();
+
+	RELEASE_LOCK(orb_lock);
 }
 
 /**
- * Set both orbs to the specified color. This function
- * is intended to be used with the predefined
- * colors. orb_init must be called before this
- * function may be used.
+ * Set the orbs to the respective values. The orbs must be initialized before this function may be used. Note that, when
+ * setting both orbs, this function is faster than calling orb1_set and orb2_set (or orb_n_set) because the values are
+ * only sorted once.
  *
- * @param col the color to set the orbs to
- *
- * @see orb_init
+ * @param red1
+ * @param green1
+ * @param blue1
+ * @param red2
+ * @param green2
+ * @param blue2
+ * @see orb1_set
+ * @see orb2_set
+ * @see orb_n_set
  **/
-void orb_set_color(int col)
-{
- int red, green, blue;
+void orbs_set (
+    uint8_t red1, uint8_t green1, uint8_t blue1,
+    uint8_t red2, uint8_t green2, uint8_t blue2) {
 
- red = ((col & 0xE0) >> 5) * 36;
- green = ((col & 0x1C) >> 2) * 36;
- blue = (col & 0x03) * 85;
+	REQUIRE_LOCK_OR_RETURN(orb_lock);
 
- orb_set(red, green, blue);
+    set_orb_values (0, red1, green1, blue1);
+    set_orb_values (1, red2, green2, blue2);
+    apply_orbs ();
+
+	RELEASE_LOCK(orb_lock);
 }
 
+
+// ******************************
+// ** Predefined color setting **
+// ******************************
+
+// This functions just call the corresponding orb*_set functions. If the orbs array is accessed in any other way, it
+// must be synchronized on orb_lock (REQUIRE_LOCK_OR_RETURN and RELEASE_LOCK)! Note that one synchronized function
+// cannot call another one with this lock implementation.
+
+// Macros for extracting a color.
+#define C_RED(col)   (((col & 0xE0) >> 5) * 36)
+#define C_GREEN(col) (((col & 0x1C) >> 2) * 36)
+#define C_BLUE(col)  (((col & 0x03)     ) * 85)
+
 /**
- * Set orb1 to the specified color. This function
- * is intended to be used with the predefined
- * colors. orb_init must be called before this
- * function may be used.
+ * Set the specified orb to the specified color. This function is intended to be used with the predefined colors.
  *
+ * @param num the number of the orb to set (0 or 1)
  * @param col the color to set the orbs to
+ **/
+void orb_n_set_color(uint8_t num, uint8_t col) {
+    orb_n_set(num, C_RED(col), C_GREEN(col), C_BLUE(col));
+}
+
+/**
+ * Set orb1 to the specified color. This function is intended to be used with the predefined colors.
  *
- * @see orb_init
+ * @param col the color to set the orbs to
  **/
-void orb1_set_color(int col)
-{
- int red, green, blue;
+void orb1_set_color(uint8_t col) {
+    orb1_set (C_RED(col), C_GREEN(col), C_BLUE(col));
+}
 
- red = ((col & 0xE0) >> 5) * 36;
- green = ((col & 0x1C) >> 2) * 36;
- blue = (col & 0x03) * 85;
-
- orb1_set(red, green, blue);
+/**
+ * Set orb2 to the specified color. This function is intended to be used with the predefined colors.
+ *
+ * @param col the color to set the orbs to
+ **/
+void orb2_set_color(uint8_t col) {
+    orb2_set(C_RED(col), C_GREEN(col), C_BLUE(col));
 }
 
 /**
- * Set orb2 to the specified color. This function
- * is intended to be used with the predefined
- * colors. orb_init must be called before this
- * function may be used.
+ * Set both orbs to the specified color. This function is intended to be used with the predefined colors.
  *
  * @param col the color to set the orbs to
+ **/
+void orb_set_color(uint8_t col) {
+    orb_set (C_RED(col), C_GREEN(col), C_BLUE(col));
+}
+
+/**
+ * Set the orbs to the respective color. This function is intended to be used with the predefined colors.
  *
- * @see orb_init
+ * @param col1 the color to set orb 1 to
+ * @param col2 the color to set orb 2 to
  **/
-void orb2_set_color(int col)
-{
- int red, green, blue;
+void orbs_set_color(uint8_t col1, uint8_t col2) {
+    orbs_set (C_RED(col1), C_GREEN(col1), C_BLUE(col1), C_RED(col2), C_GREEN(col2), C_BLUE(col2));
+}
 
- red = ((col & 0xE0) >> 5) * 36;
- green = ((col & 0x1C) >> 2) * 36;
- blue = (col & 0x03) * 85;
+#undef C_BLUE
+#undef C_GREEN
+#undef C_RED2
 
- orb2_set(red, green, blue);
+
+// ******************
+// ** Mode setting **
+// ******************
+
+/**
+ * Enables the orb timer. Note that you usually don't want to use this function directly. Instead, use orb_set_mode.
+ * @see orb_set_mode
+ **/
+void orb_enable_timer (void) {
+    // Use 8 bit TC0.
+    //
+    // Timer mode: We cannot use CTC mode because it can only clear on OCR0 (in contrast to the 16 bit timers which can
+    // also use the ICR for that) and OCR0 is already used for generating output compare interrupts. We also need
+    // immediate (non double buffered) update of OCR0, so the only mode left is "Normal".
+    //
+    // Note that for a timer counting from 0 to 255, there are 256 states and thus 257 output possibilities
+    // (0/256...256/256)! However, there are only 256 values in the byte used to specify the PWM value. Possible ways
+    // to deal with that:
+    //   1. use a 16 bit variable for the PWM value (memory waste, overhead)
+    //   2. use an additional flag for the 257th value (inconvenient)
+    //   3. use 1/256...256/256 (skip 0, never complete off)
+    //   4. use 0/256...256/256 (skip 256, never complete on)
+    //   5. skip a value somewhere in the middle
+    //   6. reload the timer after 254
+    // For this implementation, variant 4 was chosen.
+    //
+    // Using an 8 bit timer has the added advantage that all the comparisons are faster.
+    
+    // Normal mode, Compare match output off, Prescaler
+    TCCR0=_BV(CS02) | _BV(CS01); // 256, 120 Hz
+
+    // Enable the interrupts
+    TIMSK|= _BV(OCIE0) | _BV(TOIE0);
 }
 
-//DOES THIS WORK?
-// Disables the timer1 interrupt, disabling the Orb's color fading capabilities
-// You can still turn the red, green, and blue leds on and off with set_orb_dio
-/* If we use the PWM for anything else besides the ORB, this implementation needs to be done better */
 /**
- * Disables the orb color fading capabilities
- * by disabling the timer1 interrupt.
- *
- * @see orb_init
+ * Disables the orb timer. Note that you usually don't want to use this function directly. Instead, use orb_set_mode.
+ * @see orb_set_mode
  **/
-void orb_disable()
-{
-	TCCR3B &= 0;  	//Turn off everything
-	ORB_PORT |= _BV(ORB1_RED);
-	ORB_PORT |= _BV(ORB1_GREEN);
-	ORB_PORT |= _BV(ORB1_BLUE);
-	ORB_PORT |= _BV(ORB2_RED);
-	ORB_PORT |= _BV(ORB2_GREEN);
-	ORB_PORT |= _BV(ORB2_BLUE);
+void orb_disable_timer (void) {
+    // Disable the interrupts
+    TIMSK&=~( _BV(OCIE0) | _BV(TOIE0));
 }
 
-//DOES THIS WORK?
-// Enables the timer1 interrupt, enabling the Orb's color fading capabilities
+
+void orb_set_mode (orb_mode_t mode) {
+    // Set enable_orb_pwm to the appropriate value and disable or enable the timer.
+    if (mode==orb_mode_binary) {
+        orb_disable_timer ();
+
+        enable_orb_pwm=false;
+        apply_orbs ();
+    }
+    else { // orb_mode_pwm
+        enable_orb_pwm=true;
+        apply_orbs ();
+
+        orb_enable_timer ();
+    }
+}
+
+
+// ********************
+// ** Initialization **
+// ********************
+
+// Orb initialization code common to all modes.
+static void orb_init_common (void) {
+    // Enable the output ports and turn off the LEDs
+    ORBPORT |=  all_orbs_mask;
+    ORBDDR  |=  all_orbs_mask;
+
+    // Set all orbs to "off"
+    orb_set (0, 0, 0);
+    
+#ifdef LIGHTS_DEBUG
+    LIGHTS_DEBUG_INIT
+#endif
+}
+
 /**
- * Enables the orb's color fading capabilities.
- *
- * @see orb_init
+ * Initializes the orbs in PWM mode. One of the orb_init* functions must be called before the orbs can be used.
+ * 
+ * @see orb_init_pwm
  **/
-void orb_enable()
-{
-//	TCCR0 |= _BV(COM01) | _BV(COM00)  | _BV(WGM00) | _BV(CS01);	//Toggle OC Pin on match, FAST PWM Mode, clock/8
-	TCCR3B =_BV(CS31);
+void orb_init_binary (void) {
+    orb_init_common ();
+    orb_set_mode (orb_mode_binary);
 }
 
-/** @} **/ //end group
+/**
+ * Initializes the orbs in PWM mode. One of the orb_init* functions must be called before the orbs can be used.
+ * 
+ * @see orb_init_binary
+ **/
+void orb_init_pwm (void) {
+    orb_init_common ();
+    orb_set_mode (orb_mode_pwm);
+}
 
+/**
+ * Initializes the orbs in default mode. One of the orb_init* functions must be called before the orbs can be used. Use
+ * the orb_init_binary or orb_init_pwm function if you want one specific mode.
+ *
+ * @see orb_init_pwm
+ * @see orb_init_binary
+ **/
+void orb_init () {
+    orb_init_pwm ();
+}
Index: branches/encoders/code/projects/libdragonfly/ring_buffer.h
===================================================================
--- branches/encoders/code/projects/libdragonfly/ring_buffer.h	(revision 1344)
+++ branches/encoders/code/projects/libdragonfly/ring_buffer.h	(revision 1345)
@@ -1,12 +1,39 @@
-/** @file Include macros to create a ring buffer. Originally written for 15-410.
- *  Modified for use in ATMega128 serial driver
+/**
+ * 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 ring_buffer.h
+ * @brief Ring Buffer
  *
- *	@author Cornell Wright (cgwright)
- *	@author Jason P. Winters (jpwinter)
+ * Include macros to create a ring buffer. Originally written for 15-410.
+ * Modified for use in ATMega128 serial driver
  *
- *	@bugs Could not create buffer of non byte-sized elements due to modding with sizeof(queue).  
- * 		  Fixed 3/2/07, Felix Duvallet and Brad Neuman
- */
+ * @author Cornell Wright (cgwright)
+ * @author Jason P. Winters (jpwinter)
+ *
+ **/
 
 #ifndef _RING_BUFFER_H
 #define _RING_BUFFER_H
