Colony

/**

 * @file sensor_matrix.h

 * @brief Definitions for sensor matrices

 *

 * Contains functions and declarations for using sensor matrices.

 *

 * @author Brian Coltin, Colony Project, CMU Robotics Club

 **/

/**

 * @defgroup sensormatrix Sensor Matrix

 * @brief the robot sensor matrix

 *

 * These functions and structures are used for localization

 * to determine the relative directions of robots.

 *

 * @{

 **/

/**

 * @struct SensorMatrix

 *

 * A sensor matrix.

 **/

typedef struct

{

	/**

	 * The size of the sensor matrix.

	**/

	unsigned int size;

	/**

	 * The matrix. Each row represents the readings of one

	 * robot.

	 **/

	int** matrix;

	/**

	 * The element representing a robot is true if that robot

	 * is in the token ring and false otherwise.

	 **/

	int* joined;

	/**

	 * The number of robots in the token ring.

	 **/

	int numJoined;

} 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);

/**@brief Set a reading in a sensor matrix **/

void sensor_matrix_set_reading(SensorMatrix* m, int observer, int robot, int reading);

/**@brief Get a reading in a sensor matrix **/

int sensor_matrix_get_reading(SensorMatrix* m, 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);

/**@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);

/**@brief Get the number of robots which have joined the token ring **/

int sensor_matrix_get_joined(SensorMatrix* m);

/** @} **/ //end defgroup

/**

 * @file wireless.h

 * @brief Contains definitions for the wireless library.

 *

 * Contains functions for the wireless library.

 *

 * @author Brian Coltin, Colony Project, CMU Robotics Club

 **/

//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.

 **/

#define WL_MAX_PACKET_GROUPS 16

/**

 * The default PAN.

 **/

#define WL_DEFAULT_PAN 0x3331

/**

 * @defgroup wireless Wireless

 * @brief Wireless definitions.

 *

 * Contains functions and definitions for dealing with wireless functionality.<br><br>

 *

 * 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 and type are sent with each packet. When a packet

 * 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>

 *

 * This architecture allows different wireless functionality to be

 * defined and handled separately, making it simpler and more

 * efficient to take advantage of the XBee's wireless functionality.

 *

 * @{

 **/

/**

 * @struct PacketGroupHandler

 * 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.

 **/

typedef struct

{

	/**

	 * The group code for this packet group. This number

	 * must be unique. The maximum number of packet groups

	 * is defined by WL_MAX_PACKET_GROUPS.

	 **/

	unsigned int groupCode;

	/**

	 * Called every half second (not in interrupt,

	 * 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

	 * are  the group code.

	 *

	 * @param frame the last four bits of the frame

	 * @param received is true if we received an ack, 0 if

	 * we did not.

	 **/

	void (*handle_response) (int frame, int received);

	/**

	 * Called when we receive a packet from this group.

	 *

	 * @param type the packet type

	 * @param source the 16-bit address of the XBee this

	 * packet was sent from

	 * @param packet the packet received

	 * @param length the length of the packet

	 **/

	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);

/**@brief Uninitialize the wireless library **/

void wl_terminate(void);

/**@brief Perform wireless library functionality **/

void wl_do(void);

/**@brief Register a packet group with the wireless library **/

void wl_register_packet_group(PacketGroupHandler* h);

/**@brief Unregister a packet group with the wireless library **/

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);

/**@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);

/**@brief Send a packet to all robots **/

void 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);

/**@brief Get the 16-bit address of the XBee module **/

unsigned int wl_get_xbee_id(void);

/** @} **/ // end defgroup

/**

 * @file wl_token_ring.h

 * @brief Declarations for the token ring packet group

 * 

 * Contains declarations for the token ring packet group.

 *

 * @author Brian Coltin, Colony Project, CMU Robotics Club

 **/

/**

 * @defgroup tokenring Token Ring

 * @brief Wireless library token ring implementation

 *

 * This packet group is used to form a token ring, which

 * keeps track of the relative directions of the robots

 * from one another.

 *

 * @{

 **/

/**@brief Register the token ring group with the wireless library.**/

void 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));

/**@brief Join the token ring **/

void wl_token_ring_join(void);

/**@brief Leave the token ring **/

void wl_token_ring_leave(void);

/**@brief Request a BOM flash from the specified robot **/

void wl_token_request(int robot);

/**@brief Return the latest BOM reading between two robots **/

int wl_token_get_sensor_reading(int source, int dest);

/**@brief Return the latest BOM reading between us and another robot **/

int wl_token_get_my_sensor_reading(int dest);

/** @} **/ //end token ring group

//comment out this line if using a computer hooked up to an xbee

//#define ROBOT

//uncomment this line for debug information

#define WL_DEBUG

// Packet Groups and Types

// Error group

#define WL_ERROR_GROUP 1

#define WL_ERROR_STRING_TYPE 1

// Token Ring group

#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

// timing constants

#define BOM_DELAY 100

#define DEATH_DELAY 3

#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

#include <serial.h>

#endif

#ifdef ROBOT

#define WL_DEBUG_PRINT( s ) usb_puts( s )

#else

#define WL_DEBUG_PRINT( s ) printf( s )

#endif

#ifdef ROBOT

#define WL_DEBUG_PRINT_INT( i ) usb_puti(i)

#else

#define WL_DEBUG_PRINT_INT( i ) printf("%i", i)

#endif

#else

#define WL_DEBUG_PRINT( s )

#define WL_DEBUG_PRINT_INT( i )

#endif

/**

 * @file xbee.h

 * @brief Contains definitions for using the XBee

 *

 * Contains definitions for interfacing with the 

 * XBee module, from either a robot or a computer.

 * To use a robot, define ROBOT in wl_defs.h, and

 * to use a computer, don't define ROBOT.

 *

 * @author Brian Coltin, Colony Project, CMU Robotics Club

 **/

/**

 * @defgroup xbee XBee

 * @brief Interface with the XBee module

 *

 * Interface with the XBee module.

 *

 * @{

 **/

/*Definitions*/

/**@brief Do not change the PAN **/

#define XBEE_PAN_DEFAULT 0xFFFF

/**@brief Broadcast to all robots in the PAN **/

#define XBEE_BROADCAST 0xFFFF

/**@brief No special options **/

#define XBEE_OPTIONS_NONE 0x00

/**@brief Do not receive a TX_STATUS message from this packet **/

#define XBEE_OPTIONS_DISABLE_RESPONSE 0x01

/**@brief Send the packet to all PANS **/

#define XBEE_OPTIONS_BROADCAST_ALL_PANS 0x04

/**@brief A transmit status packet **/

#define XBEE_TX_STATUS 0x89

/**@brief A packet received from another XBee **/

#define XBEE_RX 0x81

/**@brief Initialize the XBee library **/

void xbee_lib_init(int pan_id);

/**@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);

/**@brief Set the PAN ID for the XBee **/

void xbee_set_pan_id(int id);

/**@brief Get the XBee's PAN ID **/

unsigned int xbee_get_pan_id(void);

/**@brief Get the XBee's 16-bit address **/

unsigned int xbee_get_address(void);

/**@}**/ //end defgroup

/**

 * @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);

/** @} **/

/**

 * @file wl_error_group.h

 * @brief A packet group for error messages.

 *

 * A packet group for sending and receiving error

 * messages.

 *

 * @author Brian Coltin, Colony Project, CMU Robotics Club

 **/

/**

 * @defgroup wlerror Error Packets

 * @brief Functions for sending and receiving error packets

 * 

 * Functions for sending and receiving error packets.

 * 

 * @{

 **/

/**@brief Register this packet group with the wireless library **/

void wl_error_register(void);

/**@brief Unregister this packet group with the wireless library **/

void wl_error_unregister(void);

/**@brief Send a string in an error packet **/

void wl_error_send_string(char* str);

/** @} **/ // end defgroup

#include <wl_token_ring.h>

#include <stdlib.h>

#include <stdio.h>

#include <wl_defs.h>

#include <wireless.h>

#include <sensor_matrix.h>

#include <queue.h>

#ifdef ROBOT

#include <bom.h>

#include <time.h>

#endif

#define DEFAULT_SENSOR_MATRIX_SIZE 20

//TODO: idea: if the xbee can handle many packets at once, we could have everyone

//send their BOM reading for a particular robot right after getting it.

/*Ring States*/

#define NONMEMBER 0

#define MEMBER 1

#define JOINING 2

#define ACCEPTED 3

#define LEAVING 4

/*Frame Types*/

#define TOKEN_JOIN_ACCEPT_FRAME 1

/*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);

/*Helper Functions*/

void wl_token_pass_token(void);

int get_token_distance(int robot1, int robot2);

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);

/*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;

//true if the robot should be in the token ring, 0 otherwise

int ringState = NONMEMBER;

//the id of the robot who accepted us into the token ring, only used in ACCEPTED state

int acceptor = -1;

//id of the robot we are accepting

int accepted = -1;

//the counter for when we assume a robot is dead

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;

#ifdef ROBOT

void (*bom_on_function) (void) = bom_on;

void (*bom_off_function) (void) = bom_off;

int (*get_max_bom_function) (void) = get_max_bom;

#else

void do_nothing(void) {}

int get_nothing(void) {return -1;}

void (*bom_on_function) (void) = do_nothing;

void (*bom_off_function) (void) = do_nothing;

int (*get_max_bom_function) (void) = get_nothing;

#endif

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};

/**

 * 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()

{

	if (wl_get_xbee_id() > 0xFF)

	{

		//Note: if this becomes an issue (unlikely), we could limit sensor information

		//to half a byte and use 12 bits for the id

		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;

	}

	sensorMatrix = sensor_matrix_create();

	interrupting = queue_create();

	//add ourselves to the sensor matrix

	sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);

	wl_register_packet_group(&wl_token_ring_handler);

}

/**

 * Removes the packet group from the wireless library.

 **/

void wl_token_ring_unregister()

{

	wl_unregister_packet_group(&wl_token_ring_handler);

}

/**

 * Sets the functions that are called when the BOM ought to be

 * 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

 * should be turned on

 * @param off_function the function to be called when the BOM

 * should be turned off

 * @param max_bom_function the function to be called when a

 * measurement of the maximum BOM reading is needed.

 **/

void wl_token_ring_set_bom_functions(void (*on_function) (void),

	void (*off_function) (void), int (*max_bom_function) (void))

{

	bom_on_function = on_function;

	bom_off_function = off_function;

	get_max_bom_function = max_bom_function;

}

/**

 * Called to cleanup the token ring packet group.

 **/

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()

{

	//someone is not responding, assume they are dead

	if (deathDelay == 0)

	{

		//pass the token to the next robot if we think someone has died

		//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);

			WL_DEBUG_PRINT("Robot ");

			WL_DEBUG_PRINT_INT(wl_token_next_robot);

			WL_DEBUG_PRINT(" has died.\r\n");

		}

		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)

		{

			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);

			ringState = MEMBER;

			//this will make us pass the token to ourself

			//repeatedly, and other robots when they join

			deathDelay = DEATH_DELAY;

			wl_token_next_robot = wl_get_xbee_id();

		}

		else

		{

			WL_DEBUG_PRINT("Attempting to join the token ring again.\r\n");

			//attempt to rejoin with a random delay

			wl_token_ring_join();

			joinDelay = rand() / (RAND_MAX / JOIN_DELAY) + 1;

		}

	}

	if (deathDelay >= 0)

		deathDelay--;

	if (joinDelay >= 0)

		joinDelay--;

}

/**

 * 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)

{

	if (!received)

	{

		WL_DEBUG_PRINT("FAILED.\r\n");

	}

}

/**

 * Called when we recieve a token ring packet.

 * @param type the type of the packet

 * @param source the id of the robot who sent the packet

 * @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)

{

	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);

			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;

		case WL_TOKEN_JOIN_ACCEPT:

			wl_token_join_accept_receive(source);

			break;

		default:

			WL_DEBUG_PRINT("Unimplemented token ring packet received.\r\n");

			break;

	}

}

/**

 * 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.

 **/

void wl_token_ring_join()

{

	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);

}

/**

 * Causes the robot to leave the token ring. The robot stops

 * alerting others of its location, but continues storing the

 * locations of other robots.

 **/

void wl_token_ring_leave()

{

	ringState = LEAVING;

}

/**

 * Requests that the specified robot be given the token and

 * allowed to flash its BOM. After its BOM is flashed, the

 * token will return to the robot who sent it.

 *

 * @param robot the ID of the robot which should flash its BOM

 **/

void wl_token_request(int robot)

{

	char buf[1];

	buf[0] = robot;

	wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_INTERRUPT_REQUEST,

		buf, 1, 0);

}

/**

 * Returns the BOM reading robot source has for robot dest.

 *

 * @param source the robot that made the BOM reading

 * @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

 **/

int wl_token_get_sensor_reading(int source, int dest)

{

	return sensor_matrix_get_reading(sensorMatrix, source, dest);

}

/**

 * 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

 **/

int wl_token_get_my_sensor_reading(int dest)

{

	return wl_token_get_sensor_reading(wl_get_xbee_id(), dest);

}

/**

 * 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

 *		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)

{

	int i, j;

	deathDelay = -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++)

	{

		if (i == source)

			continue;

		//set the sensor information we receive

		if (j < sensorDataLength / 2 && sensorData[2 * j] == i)

		{

			//the robot we were going to accept has already been accepted

			if (accepted == i)

			{

				accepted = -1;

				WL_DEBUG_PRINT("Someone accepted the robot we did.\r\n");

			}

			sensor_matrix_set_reading(sensorMatrix, source, i,

						sensorData[2 * j + 1]);

			sensor_matrix_set_in_ring(sensorMatrix, i, 1);

			j++;

		}

		else

		{

			if (sensor_matrix_get_in_ring(sensorMatrix, i))

			{

				WL_DEBUG_PRINT("Robot ");

				WL_DEBUG_PRINT_INT(i);

				WL_DEBUG_PRINT(" has been removed from 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(sensorMatrix, i, 0);

			}

			if (i == wl_get_xbee_id() && ringState == MEMBER)

			{

				sensor_matrix_set_in_ring(sensorMatrix,

						wl_get_xbee_id(), 1);

				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);

				ringState = NONMEMBER;

				acceptor = -1;

				wl_token_ring_join();

			}

		}

	}

	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();

}

/**

 * Gets the distance in the token ring between two robots.

 *

 * @param robot1 the first robot

 * @param robot2 the second robot

 *

 * @return the number of passes before the token is expected

 * to reach robot2 from robot1

 **/

int get_token_distance(int robot1, int robot2)

{

	int curr = robot1 + 1;

	int count = 1;

	while (1)

	{

		if (curr == sensor_matrix_get_size(sensorMatrix))

			curr = 0;

		if (curr == robot2)

			break;

		if (sensor_matrix_get_in_ring(sensorMatrix, curr))

			count++;

		curr++;

	}

	return count;

}

/**

 * Passes the token to the next robot in the token ring.

 **/

void wl_token_pass_token()

{

	char nextRobot;

	int i = wl_get_xbee_id() + 1;

	if (accepted == -1)

	{

		while (1)

		{

			if (i == sensor_matrix_get_size(sensorMatrix))

				i = 0;

			if (sensor_matrix_get_in_ring(sensorMatrix, i))

			{

				nextRobot = (char)i;

				break;

			}

			i++;

		}

	}

	else

	{

		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);

		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())

		{

			buf[2*j + 1] = i;

			buf[2*j + 2] = sensor_matrix_get_reading(sensorMatrix, wl_get_xbee_id(), i);

			j++;

		}

	WL_DEBUG_PRINT("Passing the token to robot ");

	WL_DEBUG_PRINT_INT(buf[0]);

	WL_DEBUG_PRINT(".\r\n");

	wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS,

		buf, packetSize, 3);

	wl_token_next_robot = nextRobot;

	deathDelay = DEATH_DELAY;

	free(buf);

}

/**

 * Called when a packet is received stating that another robot has turned

 * its BOM on. Our BOM is then read, and the data is added to the sensor

 * matrix.

 *

 * @param source the robot whose BOM is on

 **/

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(), 

		source, get_max_bom_function());

}

/**

 * 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()

{

	WL_DEBUG_PRINT("We have the token.\r\n");

	if (ringState == ACCEPTED)

	{

		sensor_matrix_set_in_ring(sensorMatrix,

			wl_get_xbee_id(), 1);

		WL_DEBUG_PRINT("Now a member of the token ring.\r\n");

		ringState = MEMBER;

	}

	if (ringState == LEAVING || ringState == NONMEMBER)

	{

		sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);

		if (ringState == NONMEMBER)

		{

			WL_DEBUG_PRINT("We should have left the token ring, but didn't.\r\n");

		}

	}

	//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);

	bom_on_function();

	#ifdef ROBOT

	delay_ms(BOM_DELAY);

	#endif

	bom_off_function();

	if (!sensor_matrix_get_in_ring(sensorMatrix, wl_get_xbee_id()))

	{

		WL_DEBUG_PRINT("Removed from sensor matrix while flashing BOM.\r\n");

	}

	wl_token_pass_token();

}

/**

 * Called when a request to join the token ring is received.

 * If we are the robot preceding the requester in the ring,

 * we respond with a JOIN_ACCEPT packet and pass the token to

 * this robot when we receive the token.

 *

 * @param source the robot who requested to join

 **/

void wl_token_join_receive(int source)

{

	WL_DEBUG_PRINT("Received joining request from robot ");

	WL_DEBUG_PRINT_INT(source);

	WL_DEBUG_PRINT(".\r\n");

	//we cannot accept the request if we are not a member

	if (ringState != MEMBER)

		return;

	//if they didn't get our response, see if we should respond again

	if (accepted == source)

		accepted = -1;

	//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;

		//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))

			return;

		i--;

	}

	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)

		wl_token_pass_token();

}

/**

 * Called when we receive a JOIN_ACCEPT packet in attempting to join

 * the token ring.

 * Our attempt to join the ring is stopped, and we wait for the token.

 *

 * @param source the robot who accepted us

 **/

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;

	ringState = ACCEPTED;

	acceptor = source;

	//add ourselves to the token ring

	sensor_matrix_set_in_ring(sensorMatrix, 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);

}

/**

 * 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);

}

#include "xbee.h"

#include "wl_defs.h"

#ifndef ROBOT

#include <fcntl.h>

#include <unistd.h>

#include <pthread.h>

#include <errno.h>

#else

#include <serial.h>

#include <avr/interrupt.h>

#endif

#include <stdio.h>

#include <stdlib.h>

#include <string.h>

#include <queue.h>

#define XBEE_FRAME_START 0x7E

/*Frame Types*/

#define XBEE_FRAME_STATUS 0x8A

#define XBEE_FRAME_AT_COMMAND 0x08

#define XBEE_FRAME_AT_COMMAND_RESPONSE 0x88

#define XBEE_FRAME_TX_REQUEST_64 0x00

#define XBEE_FRAME_TX_REQUEST_16 0x01

#define XBEE_FRAME_TX_STATUS XBEE_TX_STATUS

#define XBEE_FRAME_RX_64 0x80

#define XBEE_FRAME_RX_16 XBEE_RX

/*Internal Function Prototypes*/

/*I/O Functions*/

void xbee_send(char* buf, int size);

void xbee_send_string(char* c);

#ifndef ROBOT

void 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);

/*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);

/*Global Variables*/

#ifndef ROBOT

int xbee_stream;

pthread_t* xbee_listen_thread;

#endif

Queue* xbee_queue;

//used to store packets as they are read

char xbee_buf[128];

int currentBufPos = 0;

//XBee status

unsigned int xbee_panID = XBEE_PAN_DEFAULT;

unsigned int xbee_pending_panID = XBEE_PAN_DEFAULT;

unsigned int xbee_address = 0;

/*Function Implementations*/

#ifdef ROBOT

/**

 * Interrupt for the robot. Adds bytes received from the xbee

 * to the queue.

 **/

ISR(USART1_RX_vect)

{

	char c = UDR1;

	queue_add(xbee_queue, (void*)(int)c);

}

#else

/**

 * Thread that listens to the xbee.

 **/

void* listen_to_xbee(void* x)

{

	char c;

	while (1)

	{

		xbee_read(&c, 1);

		queue_add(xbee_queue, (void*)(int)c);

	}

	return 0;

}

#endif

/**

 * 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(int pan_id)

{

	xbee_queue = queue_create();

	#ifdef ROBOT

	//enable the receiving interrupt

	UCSR1B |= _BV(RXCIE);

	sei();

	#else

	xbee_stream = open("/dev/ttyUSB0", O_RDWR);

	if (xbee_stream == -1 || lockf(xbee_stream, F_TEST, 0) != 0)

		xbee_stream = open("/dev/ttyUSB1", O_RDWR);

	if (xbee_stream == -1 || lockf(xbee_stream, F_TEST, 0) != 0)

	{

		printf("Failed to open connection to XBee.\r\n");

		exit(0);

	}

	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);

	if (ret)

	{

		printf("Failed to create listener thread.\r\n");

		exit(0);

	}

	#endif

	xbee_enter_command_mode();

	xbee_enter_api_mode();

	xbee_exit_command_mode();

	xbee_send_read_at_command("MY");

	//wait to return until the address is set

	while (xbee_address == 0) xbee_get_packet(NULL);

	if (pan_id != XBEE_PAN_DEFAULT)

	{

		xbee_set_pan_id(pan_id);

		while (xbee_panID == XBEE_PAN_DEFAULT) xbee_get_packet(NULL);

	}

}

/**

 * Call when finished using the XBee library. This releases

 * all sued resources.

 **/

void xbee_terminate()

{

	#ifndef ROBOT

	pthread_cancel(*xbee_listen_thread);

	free(xbee_listen_thread);

	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)

{

	#ifdef ROBOT

	int i;

	for (i = 0; i < size; i++)

		xbee_putc(buf[i]);

	#else

	int ret = write(xbee_stream, buf, size);

	//success

	if (ret == size)

		return;

	if (ret == -1)

	{

		//interrupted by system signal, probably timer interrupt.

		//just try again

		if (errno == 4)

		{

			xbee_send(buf, size);

			return;

		}

		printf("Failed to write to xbee, error %i.\r\n", errno);

		return;

	}

	//write was interrupted after writing ret bytes

	xbee_send(buf + ret, size - ret);

	#endif

}

/**

 * Sends a string to the XBee.

 *

 * @param c the string to send to the XBEE

 **/

void xbee_send_string(char* c)

{

	xbee_send(c, strlen(c));

}

#ifndef ROBOT

void xbee_read(char* buf, int size)

{

	if (read(xbee_stream, buf, size) == -1)

		printf("Failed to read from xbee.\r\n");

}

#endif

/**

 * Enter into command mode.

 **/

void xbee_enter_command_mode()

{

	xbee_send_string("+++");

	xbee_wait_for_ok();

}

/**

 * Exit from command mode.

 **/

void xbee_exit_command_mode()

{

	xbee_send_string("ATCN\r");

	xbee_wait_for_ok();

}

/**

 * Enter API mode.

 **/

void xbee_enter_api_mode()

{

	xbee_send_string("ATAP 1\r");

	xbee_wait_for_ok();

}

/**

 * Wait until the string "OK\r" is received from the XBee.

 **/

void xbee_wait_for_ok()

{

	xbee_wait_for_string("OK\r", 3);

}

/**

 * Delay until the specified string is received from

 * the XBee. Discards all other XBee data.

 *

 * @param s the string to receive

 * @param len the length of the string

 **/

void 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;

	}

}

/**

 * Verifies that the packets checksum is correct.

 * (If the checksum is correct, the sum of the bytes

 * is 0xFF.)

 *

 * @param packet the packet received. This includes the first