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

Revision 793

Updated bay board wireless library... Doesn't work at all.

     #include <sensor_matrix.h>
     #ifdef ROBOT
     	#ifndef FIREFLY
     		#ifdef BAYBOARD
     			#include <lbom.h>
     		#else
     			#include <bom.h>
     		#endif
     	#endif
     #ifndef FIREFLY
     #include <bom.h>
     #endif
     #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*/
-...
     static void wl_token_get_token(void);
     /*Packet Handling Routines*/
     static void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength);
     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
     static SensorMatrix* sensorMatrix;
     //the robot we are waiting to say it has received the token. -1 if unspecified
     static int wl_token_next_robot = -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
     #ifdef BAYBOARD
     static void (*bom_on_function) (void) = bom_on;
     static void (*bom_off_function) (void) = bom_off;
     static int (*get_max_bom_function) (void) = get_nothing;
     #else
     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;
     #endif
     #else
     static void (*bom_on_function) (void) = do_nothing;
     static void (*bom_off_function) (void) = do_nothing;
-...
     #endif
     static PacketGroupHandler wl_token_ring_handler =
     	{wl_token_ring_timeout_handler,
     	{WL_TOKEN_RING_GROUP, wl_token_ring_timeout_handler,
     		wl_token_ring_response_handler, wl_token_ring_receive_handler,
        wl_token_ring_cleanup, WL_TOKEN_RING_GROUP};
     		wl_token_ring_cleanup};
     /**
      * Causes the robot to join an existing token ring, or create one
-...
      * 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;
-...
     		return -1;
+    	}
     	sensorMatrix = sensor_matrix_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);
-...
     /**
      * 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);
-...
     /**
      * Called to cleanup the token ring packet group.
      **/
     //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);
+    }
     /**
-...
     		//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");
-...
     	//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
-...
     	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);
-...
+    {
     	if (wl_token_is_robot_in_ring(dest) &&
     			(source == wl_get_xbee_id() || wl_token_is_robot_in_ring(source))) {
     		return sensor_matrix_get_reading(sensorMatrix, source, dest);
     		return sensor_matrix_get_reading(source, dest);
+    	}
     	return -1;
-...
      * @return a BOM reading from us to robot dest, in the range
      * 0-15, or -1 if it is unkown
      **/
     //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)
+    {
     	return wl_token_get_sensor_reading(wl_get_xbee_id(), dest);
-...
+     *
      * @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_robots_in_ring(void)
+    {
     	return sensor_matrix_get_joined(sensorMatrix);
     	return sensor_matrix_get_joined();
+    }
     /**
-...
      * @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(sensorMatrix, robot);
     	return sensor_matrix_get_in_ring(robot);
+    }
     /**
-...
     void wl_token_iterator_begin(void)
+    {
     	int i = 0;
     	while (!sensor_matrix_get_in_ring(sensorMatrix, i) && 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
     	while (!sensor_matrix_get_in_ring(i) && i < sensor_matrix_get_size()) {
     		i++;
+    	}
     	if (i == sensor_matrix_get_size(sensorMatrix)) {
       //TODO: if you do the above comment, then you can compare this to the variable also
     	if (i == sensor_matrix_get_size()) {
     		i = -1;
+    	}
-...
+     *
      * @see wl_token_iterator_begin, wl_token_iterator_next
      **/
     //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 iteratorCount != -1;
-...
     		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(sensorMatrix, iteratorCount)
     		&& iteratorCount < sensor_matrix_get_size(sensorMatrix)) {
     	while (!sensor_matrix_get_in_ring(iteratorCount)
     		&& iteratorCount < sensor_matrix_get_size()) {
     		iteratorCount++;
+    	}
     	if (iteratorCount == sensor_matrix_get_size(sensorMatrix)) {
       //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 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(sensorMatrix);
     	return sensor_matrix_get_joined();
+    }
     /**
-...
+     *
      * @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(sensorMatrix);
     	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
      */
     static 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;
     	// this prevents two tokens from being passed around at a time (second clause is in case we are joining)
     	if (source != wl_token_next_robot && bom_on_count <= DEATH_DELAY / 2 &&
     		ringState != ACCEPTED)
+    	{
     		WL_DEBUG_PRINT("Received token pass when a robot should not have died yet.\n");
     		WL_DEBUG_PRINT("There are probably two tokens going around, packet ignored.\n");
     		return;
+    	}
     	bom_on_count = -1;
     	deathDelay = -1;
     	WL_DEBUG_PRINT("Received the token from robot");
     	WL_DEBUG_PRINT_INT(source);
     	WL_DEBUG_PRINT(", next robot is ");
     	WL_DEBUG_PRINT_INT((int)nextRobot);
     	WL_DEBUG_PRINT(" \r\n");
     	sensor_matrix_set_in_ring(sensorMatrix, source, 1);
     	sensor_matrix_set_in_ring(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) {
     			continue;
-...
     				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)
-...
     			//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);
+    }
     /**
-...
     	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++;
+    	}
-...
      **/
     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;
-...
+    	{
     		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");
     		free(buf);
     		return -1;
+    	}
     	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");
     	if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, buf, packetSize, 3) != 0) {
     		free(buf);
     	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;
+    }
-...
     	bom_on_count = 0;
     	sensor_matrix_set_reading(sensorMatrix, wl_get_xbee_id(),
     	sensor_matrix_set_reading(wl_get_xbee_id(),
     		source, get_max_bom_function());
+    }
-...
     	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");
-...
     	#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;
-...
     	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--;
+    	}
-...
     	WL_DEBUG_PRINT(" into the token ring.\r\n");
     	// the token ring has not started yet
     	if (sensor_matrix_get_joined(sensorMatrix) == 1)
     	if (sensor_matrix_get_joined() == 1)
     		wl_token_pass_token();
+    }
-...
     	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);
+    }

Also available in: Unified diff

Project

General

Profile

Colony

Revision 793