Colony

#include "wl_charging_station.h"

#include <wireless.h>

#include <wl_defs.h>

#include <serial.h>

#include "charging_defs.h"

#include "charging.h"

#define NULL 0

//Possible states for the robot

/** The robot is not attempting to recharging. **/

#define NOT_RECHARGING 0

/** Waiting for an available charging station. **/

#define POLLING 1

/** Requesting to dock with a charging station. **/

#define REQUESTING 2

/** Traveling to a charging station. **/

#define SEEKING 3

/** Docked with a charging station. **/

#define DOCKED 4

/** Leaving a charging station. **/

#define DEPARTING 5

// Function prototypes

void wl_charging_timeout_handler(void);

void wl_charging_response_handler(int frame, int received);

void wl_charging_handle_receive(char type, int source, unsigned char* packet,

                                                        int length);

void wl_charging_cleanup(void);

// messgae handlers

void wl_charging_poll(int source);

void wl_charging_request(int source);

void wl_charging_verify(int source);

void wl_charging_cancel(int source);

void wl_charging_seeking(int source);

void wl_charging_docked(int source);

// sending packets

void wl_charging_send_station_available(int dest);

void wl_charging_send_request_accept(int dest);

void wl_charging_send_verify(int dest);

void wl_charging_send_cancel(int dest);

void wl_charging_send_seeking(int dest);

void wl_charging_send_docked(int dest);

void wl_charging_send_battery(int dest);

// the handler

PacketGroupHandler wl_charging_handler =

                {WL_RECHARGE_GROUP, wl_charging_timeout_handler,

                wl_charging_response_handler, wl_charging_handle_receive,

                wl_charging_cleanup};

//frames

#define STATION_AVAILABLE_FRAME 1

#define REQUEST_ACCEPT_FRAME 2

#define VERIFY_FRAME 3

#define CANCEL_FRAME 4

#define SEEKING_FRAME 5

#define DOCKED_FRAME 6

#define BATTERY_FRAME 7

/**

 * Register this packet group with the wireless library.

 * This function must be called before any other wl_charging

 * function. Also, wl_token_ring_register must be called

 * before this function, along with wl_token_ring_join.

 *

 * @see wl_charging_unregister

 **/

void wl_station_register(void)

{

        wl_register_packet_group(&wl_charging_handler);

}

/**

 * Called to unregister this packet group with the wireless

 * library.

 *

 * @see wl_charging_register

 **/

void wl_station_unregister(void)

{

        wl_unregister_packet_group(&wl_charging_handler);

}

/**

 * Sends a packet alerting a specific robot that the station

 * is available.

 *

 * @param dest the robot to tell that the station is available

 **/

void wl_charging_send_station_available(int dest)

{

        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP,

                WL_RECHARGE_STATION_AVAILABLE, NULL, 0, dest,

                STATION_AVAILABLE_FRAME);

}

/**

 * Sends a packet accepting the request of a specific robot.

 *

 * @param dest the robot to alert of its acceptance

 **/

void wl_charging_send_request_accept(int dest)

{

        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP,

                WL_RECHARGE_REQUEST_ACCEPT, NULL, 0, dest,

                REQUEST_ACCEPT_FRAME);

}

/**

 * Sends a request to verify what the robot

 * believes it is doing.

 *

 * @param dest the robot to check

 **/

void wl_charging_send_verify(int dest)

{

        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_VERIFY,

                                NULL, 0, dest, VERIFY_FRAME);

}

/**

 * Sends a packet to the robot that we are

 * no longer allowing it to charge.

 *

 * @param dest the robot to tell that it cannot charge

 **/

void wl_charging_send_cancel(int dest)

{

        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_CANCEL,

                                NULL, 0, dest, CANCEL_FRAME);

}

/**

 * Sends a packet to the robot that it may

 * attempt to dock with us.

 * 

 * @param dest the robot attempting to dock

 **/

void wl_charging_send_seeking(int dest)

{

        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_SEEKING,

                                NULL, 0, dest, SEEKING_FRAME);

}

/**

 * Sends a packet to the robot that

 * we believe it is docked with us.

 *

 * @param dest the robot that has docked with us

 **/

void wl_charging_send_docked(int dest)

{

        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_DOCKED,

                                NULL, 0, dest, DOCKED_FRAME);

}

/**

 * Sends a packet to request for the robot's battery reading

 *

 * @param dest the robot to request

 **/

void wl_charging_send_battery(int dest)

{

        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_BATTERY, NULL, 0, dest, BATTERY_FRAME);

}

/**

 * Called when this packet is unregistered with the wireless library.

 **/

void wl_charging_cleanup(void)

{

}

/**

 * Called when the timer goes off in the wireless library.

 **/

void wl_charging_timeout_handler(void)

{

        charging_robot_iterator_init();

        while (charging_robot_iterator_has_next())

        {

                int robot = charging_robot_iterator_next();

                int c = charging_get_verify_count(robot);

                if (c <= 0)

                {

                        STATION_DEBUG_PRINT("Robot ");

                        STATION_DEBUG_PUTI(robot);

                        STATION_DEBUG_PRINT(" has timed out.");

                        charging_cancel(robot);

                        continue;

                }

                if (c % VERIFY_FREQUENCY == 0 && c != VERIFY_DELAY)

                {

                        STATION_DEBUG_PRINT("Verification sent to robot ");

                        STATION_DEBUG_PUTI(robot);

                        STATION_DEBUG_PRINT(".\n");

                        wl_charging_send_verify(robot);

                }

                charging_set_verify_count(robot, c - 1);

        }

}

/**

 * Called when a response is received regarding a packet that was sent.

 * 

 * @param frame the frame number for the packet

 * @param received true if the packet was received, false otherwise

 **/

void wl_charging_response_handler(int frame, int received)

{

        if (received)

                return;

        if (!received)

        {

                STATION_DEBUG_PRINT("Sent packet not recieved.\n");

                switch (frame)

                {

                        case STATION_AVAILABLE_FRAME:

                                break;

                        case REQUEST_ACCEPT_FRAME:

                                break;

                        case VERIFY_FRAME:

                                break;

                        case CANCEL_FRAME:

                        case SEEKING_FRAME:

                        case DOCKED_FRAME:

                                break;

                }

        }

}

/**

 * Handles receiving an error packet.

 *

 * @param type the packet type

 * @param source the 16-bit address of the packet's sender

 * @param packet the packet data

 * @param length the length in bytes of the packet

 **/

void wl_charging_handle_receive(char type, int source, unsigned char* packet,

                                                        int length)

{

        switch (type)

        {

                case WL_RECHARGE_POLL_STATIONS:

                        wl_charging_poll(source);

                        break;

                case WL_RECHARGE_STATION_AVAILABLE:

                        STATION_DEBUG_PRINT("Stations should not receive ");

                        STATION_DEBUG_PRINT("station available packets.\n");

                        break;

                case WL_RECHARGE_REQUEST:

                        wl_charging_request(source);

                        break;

                case WL_RECHARGE_REQUEST_ACCEPT:

                        STATION_DEBUG_PRINT("Stations should not receive ");

                        STATION_DEBUG_PRINT("request accept packets.\n");

                        break;

                case WL_RECHARGE_VERIFY:

                        wl_charging_verify(source);

                        break;

                case WL_RECHARGE_CANCEL:

                        wl_charging_cancel(source);

                        break;

                case WL_RECHARGE_SEEKING:

                        wl_charging_seeking(source);

                        break;

                case WL_RECHARGE_DOCKED:

                        wl_charging_docked(source);

                        break;

                case WL_RECHARGE_BATTERY:

                        wl_charging_battery(source,packet,length);

                        break;

                default:

                        STATION_DEBUG_PRINT("Error packet of unknown type received.\n");

                        break;

        }

}

/**

 * Called when we receive a packet alerting us that

 * a robot is looking for stations.

 *

 * @param source the robot requesting a station

 **/

void wl_charging_poll(int source)

{

        if (charging_is_space_available() && !charging_is_robot_seeking())

                wl_charging_send_station_available(source);

        wl_charging_send_battery(source);

}

/**

 * Called when we receive a request to charge.

 *

 * @param source the robot requesting to charge

 **/

void wl_charging_request(int source)

{

        if (charging_is_space_available() && !charging_is_robot_seeking())

        {

                charging_begin_seeking(source);

                wl_charging_send_request_accept(source);

        }

        else

                wl_charging_send_cancel(source);

}

/**

 * Called when we receive a request to verify our status

 * in the docking procedure.

 *

 * @param source the robot verifying our status

 **/

void wl_charging_verify(int source)

{

        int state = charging_get_robot_state(source);

        STATION_DEBUG_PRINT("Received verify request from robot ");

        STATION_DEBUG_PUTI(source);

        STATION_DEBUG_PRINT(".\n");

        switch (state)

        {

                case STATE_SEEKING:

                        wl_charging_send_seeking(source);

                        break;

                case STATE_DOCKED:

                        wl_charging_send_docked(source);

                        break;

                case STATE_NOT_CHARGING:

                        STATION_DEBUG_PRINT("Received verify from unknown robot ");

                        STATION_DEBUG_PUTI(source);

                        STATION_DEBUG_PRINT(".\n");

                        wl_charging_send_cancel(source);

                        break;

                default:

                        STATION_DEBUG_PRINT("Unknown robot state.\n");

                        break;

        }

}

/**

 * Called when we receive a packet cancelling our

 * docking procedure.

 *

 * @param source the robot cancelling the procedure

 **/

void wl_charging_cancel(int source)

{

        int state = charging_get_robot_state(source);

        STATION_DEBUG_PRINT("Robot ");

        STATION_DEBUG_PUTI(source);

        STATION_DEBUG_PRINT(" has cancelled charging.\n");

        if (state == STATE_NOT_CHARGING)

        {

                STATION_DEBUG_PRINT("Received cancel from unknown robot.\n");

                return;

        }

        charging_set_verify_count(source, VERIFY_DELAY);

        charging_cancel(source);

}

/**

 * Called when we receive a packet stating that the

 * robot is seeking us.

 *

 * @param source the robot seeking us

 **/

void wl_charging_seeking(int source)

{

        int state = charging_get_robot_state(source);

        STATION_DEBUG_PRINT("Robot ");

        STATION_DEBUG_PUTI(source);

        STATION_DEBUG_PRINT(" has confirmed it is seeking.\n");

        if (state == STATE_NOT_CHARGING)

        {

                STATION_DEBUG_PRINT("Received seeking from unknown robot.\n");

                wl_charging_send_cancel(source);

                return;

        }

        charging_set_verify_count(source, VERIFY_DELAY);

        if (state == STATE_DOCKED)

        {

                STATION_DEBUG_PRINT("Should not have revert from docked to seeking.\n");

                if (charging_is_robot_seeking())

                {

                        charging_cancel(source);

                        wl_charging_send_cancel(source);

                }

                else

                        charging_begin_seeking(source);

        }

}

/**

 * Called when we receive a packet stating that

 * a robot is docked with us.

 * 

 * @param source the robot that is docked with us

 **/

void wl_charging_docked(int source)

{

        int state = charging_get_robot_state(source);

        STATION_DEBUG_PRINT("Robot ");

        STATION_DEBUG_PUTI(source);

        STATION_DEBUG_PRINT(" has confirmed it is docked.\n");

        if (state == STATE_NOT_CHARGING)

        {

                STATION_DEBUG_PRINT("Received docked from unknown robot.\n");

                return;

        }

        charging_set_verify_count(source, VERIFY_DELAY);

        //seeking has ended

        if (state == STATE_SEEKING)

                charging_dock(source);

}

/**

 * Called when we recieve a packet with a battery reading

 *

 * @param source the robot that is sending the battery reading

 **/

void wl_charging_battery(int source, unsigned char* packet, int length)

{

        if (charging_get_robot_state(source)==STATE_NOT_CHARGING)

                check_battery(source, (int)packet[0]);

        else

                update_battery(source, (int)packet[0]);

}