Colony

/**

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

#include <wl_defs.h>

#include <wireless.h>

#include <sensor_matrix.h>

#ifdef ROBOT

#ifndef FIREFLY

#include <bom.h>

#endif

#include <time.h>

#endif

#define DEFAULT_SENSOR_MATRIX_SIZE 20

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

#define WL_TOKEN_PASS_FRAME        2

/*Function Prototypes*/

/*Wireless Library Prototypes*/

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

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

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 robot we are waiting to say it has received the token. -1 if unspecified

static int wl_token_next_robot = -1;

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

static int ringState = NONMEMBER;

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

static int acceptor = -1;

//id of the robot we are accepting

static int accepted = -1;

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

static int deathDelay = -1;

//the counter for joining, before we form our own token ring

static int joinDelay = -1;

//current robot to check in the iterator

static int iteratorCount = 0;

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

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

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

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

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.

 **/

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.

 **/

int 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 -1;

        }

        sensor_matrix_create();

        //add ourselves to the sensor matrix

        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.

 **/

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.

 **/

static void wl_token_ring_cleanup()

{

}

/**

 * Called approximately every quarter second by the wireless library.

 **/

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

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

                {

                        WL_DEBUG_PRINT("Creating our own token ring, no robots seem to exist.\r\n");

                        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

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

        }

        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

 **/

static 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

 **/

static void wl_token_ring_receive_handler(char type, int source, unsigned char* packet, int length)

{

        switch (type)

        {

                case WL_TOKEN_PASS:

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

        }

}

/**

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

{

        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;

}

/**

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

}

/**

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

}

/**

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

        while (!sensor_matrix_get_in_ring(i) && i < sensor_matrix_get_size()) {

                i++;

        }

        if (i == sensor_matrix_get_size()) {

                i = -1;

        }

        iteratorCount = i;

}

/**

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

                && iteratorCount < sensor_matrix_get_size()) {

                iteratorCount++;

        }

        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

 **/

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

 **/

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

 */

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

        //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(); 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(source, i,

                                                sensorData[2 * j + 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(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(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(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;

                }

                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;

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

}

/**

 * 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

 **/

static int get_token_distance(int robot1, int robot2)

{

        int curr = robot1 + 1;

        int count = 1;

        while (1)

        {

                if (curr == sensor_matrix_get_size())

                        curr = 0;

                if (curr == robot2)

                        break;

                if (sensor_matrix_get_in_ring(curr))

                        count++;

                curr++;

        }

        return count;

}

/**

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

 **/

static int wl_token_pass_token()

{

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

                                i = 0;

                        }

                        if (sensor_matrix_get_in_ring(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(accepted, 1);

                nextRobot = accepted;

                accepted = -1;

        }

        int j = 0;

        for (i = 0; i < sensor_matrix_get_size(); i++) {

                if (sensor_matrix_get_in_ring(i) && i != wl_get_xbee_id())

                {

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

        WL_DEBUG_PRINT(".\r\n");

        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;

        return 0;

}

/**

 * 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

 **/

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

        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.

 **/

static void wl_token_get_token()

{

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

        if (ringState == ACCEPTED)

        {

                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(wl_get_xbee_id(), 0);

                if (ringState == NONMEMBER)

                {

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

                }

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

        {

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

                return;

        }

        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

 **/

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

        // the token ring has not started yet

        if (sensor_matrix_get_joined() == 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

 **/

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 = JOIN_DELAY;

        ringState = ACCEPTED;

        acceptor = source;

        //add ourselves to the token ring

        sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);

}