/branches/encoders/code/projects/libwireless/lib/wl_token_ring.c - Diff - Colony - Robotics Club

Revision 1345

Final spline code for master/slave

updated outdated libdragonfly and libwireless

     /**
      * 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>
-...
     #include <wl_defs.h>
     #include <wireless.h>
     #include <sensor_matrix.h>
     #include <queue.h>
     #ifdef ROBOT
     #ifndef FIREFLY
-...
     #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*/
-...
     #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)
+    	{
-...
     		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);
-...
     /**
      * 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
-...
     /**
      * 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)
-...
     		//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
-...
+    		}
+    	}
     	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)
+    	{
-...
      * @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;
-...
+    }
     /**
      * 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
      *		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)
+    		{
-...
     				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);
-...
     				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)
+    			{
     				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();
+    			}
+    		}
+    	}
     	// 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);
+    }
     /**
-...
      * @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++;
+    	}
-...
     /**
      * 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++;
+    		}
+    	}
-...
+    	{
     		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;
+    }
     /**
-...
+     *
      * @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());
+    }
-...
      * 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");
+    		}
     		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
     	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();
+    }
-...
+     *
      * @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);
-...
     	//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--;
+    	}
     	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();
+    }
-...
+     *
      * @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);
+    }

Also available in: Unified diff

Project

General

Profile

Colony

Revision 1345