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

                #ifdef BAYBOARD

                        #include <lbom.h>

                #else

                        #include <bom.h>

                #endif

        #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

/*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, char nextRobot, 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;

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

static void do_nothing(void) {}

static int get_nothing(void) {return -1;}

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

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;

        }

        sensorMatrix = sensor_matrix_create();

        //add ourselves to the sensor matrix

        sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);

        wl_register_packet_group(&wl_token_ring_handler);

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

{

        sensor_matrix_destroy(sensorMatrix);

}

/**

 * 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(sensorMatrix, wl_token_next_robot, 0);

                        WL_DEBUG_PRINT("Robot ");

                        WL_DEBUG_PRINT_INT(wl_token_next_robot);

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

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

                {

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

                        sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);

                        ringState = MEMBER;

                        //this will make us pass the token to ourself

                        //repeatedly, and other robots when they join

                        deathDelay = DEATH_DELAY;

                        wl_token_next_robot = wl_get_xbee_id();

                }

                else

                {

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

                        //attempt to rejoin with a random delay

                        wl_token_ring_join();

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

                }

        }

        if (deathDelay >= 0) {

                deathDelay--;

        }

        if (joinDelay >= 0) {

                joinDelay--;

        }

        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:

                        if (length < 1)

                        {

                                WL_DEBUG_PRINT("Malformed Token Pass packet received.\r\n");

                                return;

                        }

                        wl_token_pass_receive(source, packet[0], packet + 1, length - 1);

                        break;

                case WL_TOKEN_BOM_ON:

                        //add the robot to the sensor matrix if it is not already there

                        wl_token_bom_on_receive(source);

                        break;

                case WL_TOKEN_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(sensorMatrix, 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(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)