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

Revision 340

We're ready to do a two robot autonomous recharging video with run around, aside from some docking problems. There seem to be wireless issues with three robots, however.

     /**
      * 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
-...
     /*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);
-...
     int deathDelay = -1;
     //the counter for joining, before we form our own token ring
     int joinDelay = -1;
     //queue containing ids of interruption requests
     Queue* interrupting = NULL;
     //current robot to check in the iterator
     int iteratorCount = 0;
     // the amount of time a robot has had its BOM on for
     int bom_on_count = 0;
     void do_nothing(void) {}
     int get_nothing(void) {return -1;}
-...
+    	}
     	sensorMatrix = sensor_matrix_create();
     	interrupting = queue_create();
     	//add ourselves to the sensor matrix
     	sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);
-...
     void wl_token_ring_cleanup()
+    {
     	sensor_matrix_destroy(sensorMatrix);
     	queue_destroy(interrupting);
+    }
     /**
-...
     			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
-...
     		deathDelay--;
     	if (joinDelay >= 0)
     		joinDelay--;
     	if (bom_on_count >= 0)
     		bom_on_count++;
+    }
     /**
-...
     			//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;
-...
+    {
     	WL_DEBUG_PRINT("Joining the token ring.\r\n");
     	ringState = JOINING;
     	joinDelay = JOIN_DELAY;
     	joinDelay = DEATH_DELAY * 2;
     	wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN,
     		NULL, 0, 0);
+    }
-...
+    }
     /**
      * 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
-...
      **/
     int wl_token_get_sensor_reading(int source, int dest)
+    {
     	return sensor_matrix_get_reading(sensorMatrix, source, dest);
     	if (wl_token_is_robot_in_ring(dest) &&
     			(source == wl_get_xbee_id() || wl_token_is_robot_in_ring(source)))
     		return sensor_matrix_get_reading(sensorMatrix, source, dest);
     	return -1;
+    }
     /**
-...
     void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength)
+    {
     	int i, j;
     	// this prevents two tokens from being passed around at a time (second clause is in case we are joining)
     	if (source != wl_token_next_robot && bom_on_count <= DEATH_DELAY / 2 &&
     		ringState != ACCEPTED)
+    	{
     		WL_DEBUG_PRINT("Received token pass when a robot should not have died yet.\n");
     		WL_DEBUG_PRINT("There are probably two tokens going around, packet ignored.\n");
     		return;
+    	}
     	bom_on_count = -1;
     	deathDelay = -1;
     	WL_DEBUG_PRINT("Received the token, next robot is ");
     	WL_DEBUG_PRINT("Received the token from robot");
     	WL_DEBUG_PRINT_INT(source);
     	WL_DEBUG_PRINT(", next robot is ");
     	WL_DEBUG_PRINT_INT((int)nextRobot);
     	WL_DEBUG_PRINT(" \r\n");
     	sensor_matrix_set_in_ring(sensorMatrix, source, 1);
-...
     	WL_DEBUG_PRINT("Robot ");
     	WL_DEBUG_PRINT_INT(source);
     	WL_DEBUG_PRINT(" has flashed its bom.\r\n");
     	bom_on_count = 0;
     	sensor_matrix_set_reading(sensorMatrix, wl_get_xbee_id(),
     		source, get_max_bom_function());
+    }
-...
     			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)
-...
     		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);
-...
     	if (!sensor_matrix_get_in_ring(sensorMatrix, wl_get_xbee_id()))
+    	{
     		WL_DEBUG_PRINT("Removed from sensor matrix while flashing BOM.\r\n");
     		return;
+    	}
     	wl_token_pass_token();
-...
     	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();
-...
     	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;
-...
+    }
     /**
      * 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
-...
     		i++;
     	if (i == sensor_matrix_get_size(sensorMatrix))
     		i = -1;
     	iteratorCount = i;
+    }
     /**
-...
      **/
     int wl_token_iterator_has_next(void)
+    {
     	return iteratorCount == -1;
     	return iteratorCount != -1;
+    }
     /**
-...
+    }
     /**
      * Called when we receive a request to interrupt the token ring.
      * We add the robot to our list of interrupt requests,
      * and will send the token to this robot when we next receive the
      * token, unless someone else does so first.
      * Returns the number of robots currently in the token ring.
+     *
      * @param source the robot requesting interruption
      * @param robt the robot requested to interrupt the token ring
      * @return the number of robots in the token ring
      **/
     void wl_token_interrupt_request_receive(int source, int robot)
     int wl_token_get_num_robots(void)
+    {
     	queue_add(interrupting, (void*)robot);
     	return sensor_matrix_get_joined(sensorMatrix);
+    }
     int wl_token_get_num_robots(void){
       return sensor_matrix_get_joined(sensorMatrix);
     /**
      * Returns the number of robots in the sensor matrix.
+     *
      * @return the number of robots in the sensor matrix
      **/
     int wl_token_get_matrix_size(void)
+    {
     	return sensor_matrix_get_size(sensorMatrix);
+    }
     int wl_token_get_matrix_size(void){
       return sensor_matrix_get_size(sensorMatrix);
+    }

Also available in: Unified diff

Project

General

Profile

Colony

Revision 340