Colony

#include "wl_recharge_group.h"

#include <wireless.h>

#include <wl_defs.h>

#include <battery.h>

#include <serial.h>

#include "recharge_defs.h"

// Function prototypes

void wl_recharge_timeout_handler(void);

void wl_recharge_response_handler(int frame, int received);

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

                                                        int length);

void wl_recharge_cleanup(void);

// messgae handlers

void wl_recharge_available_station(int source);

void wl_recharge_request_accepted(int source);

void wl_recharge_verify(int source);

void wl_recharge_cancel(int source);

void wl_recharge_seeking(int source);

void wl_recharge_docked(int source);

void wl_recharge_battery_request(int source);

// sending packets

void wl_recharge_send_poll(void);

void wl_recharge_send_request(int dest);

void wl_recharge_send_verify(int dest);

void wl_recharge_send_cancel(int dest);

void wl_recharge_send_seeking(int dest);

void wl_recharge_send_docked(int dest);

void wl_recharge_send_battery(int dest);

// the handler

PacketGroupHandler wl_recharge_handler =

                {WL_RECHARGE_GROUP, wl_recharge_timeout_handler,

                wl_recharge_response_handler, wl_recharge_handle_receive,

                wl_recharge_cleanup};

//timing

//timer ticks between polls

#define POLL_DELAY 4

#define REQUEST_DELAY 6

#define VERIFY_DELAY 32

#define VERIFY_FREQUENCY 11

//frames

#define REQUEST_FRAME 1

#define VERIFY_FRAME 2

#define CANCEL_FRAME 3

#define SEEKING_FRAME 4

#define DOCKED_FRAME 5

//global varaibles

//state variables

int recharge_state = NOT_RECHARGING;

int station = -1;

//counter variables

int pollCount = 0;

int requestCount = 0;

int verifyCount = 0;

/**

 * Register this packet group with the wireless library.

 * This function must be called before any other wl_recharge

 * function. Also, wl_token_ring_register must be called

 * before this function.

 *

 * @see wl_recharge_unregister

 **/

void wl_recharge_register(void)

{

        wl_register_packet_group(&wl_recharge_handler);

}

/**

 * Called to unregister this packet group with the wireless

 * library.

 *

 * @see wl_recharge_register

 **/

void wl_recharge_unregister(void)

{

        wl_unregister_packet_group(&wl_recharge_handler);

}

/**

 * Begin the procedure to charge.

 **/

void wl_recharge_begin(void)

{

        RECHARGE_DEBUG_PRINT("Beginning to recharge.\n");

        recharge_state = POLLING;

        station = -1;

        pollCount = 0;

}

/**

 * Begin the procedure to stop charging. If we are currently docked,

 * we wait for departureDelay before alerting the station of our

 * departure. This is so that we have time to leave.

 **/

void wl_recharge_stop()

{

        recharge_state = NOT_RECHARGING;

        station = -1;

}

/**

 * Call when the robot has docked with the charging station.

 * This changes the state to DOCKED.

 **/

void wl_recharge_dock(void)

{

        if (recharge_state != SEEKING)

        {

                RECHARGE_DEBUG_PRINT("Unexpected docking occurred.\n");

                return;

        }

        recharge_state = DOCKED;

}

/**

 * Call this when the robot has departed from the charging

 * station. This changes the state to DEPARTING.

 **/

void wl_recharge_depart(void)

{

        if (recharge_state != DOCKED)

        {

                RECHARGE_DEBUG_PRINT("Unexpected departure occurred.\n");

                return;

        }

        recharge_state = DEPARTING;

}

/**

 * Returns the current state of the robot, as one of the constants

 * defined for a state -

 * NOT_CHARGING, POLLING, REQUESTING, SEEKING, DOCKED, or DEPARTING.

 *

 * @return the current state of the robot

 **/

int wl_recharge_get_state(void)

{

        return recharge_state;

}

/**

 * Returns the station we are currently homing to, or -1

 * if no such station exists.

 *

 * @return the 16-bit XBee address of the charging station

 **/

int wl_recharge_get_station(void)

{

        return station;

}

/**

 * Sends a request polling for charging stations.

 **/

void wl_recharge_send_poll(void)

{

        wl_send_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_POLL_STATIONS,

                                0, 0, 0);

}

/**

 * Sends a request to seek a specific charging station.

 *

 * @param dest the charging station to seek

 **/

void wl_recharge_send_request(int dest)

{

        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_REQUEST,

                                0, 0, dest, REQUEST_FRAME);

}

/**

 * Sends a request to verify what the charging station

 * believes we are doing.

 *

 * @param dest the charging station to check

 **/

void wl_recharge_send_verify(int dest)

{

        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_VERIFY,

                                0, 0, dest, VERIFY_FRAME);

}

/**

 * Sends a packet to the charging station that we are

 * no longer attempting to charge from it.

 **/

void wl_recharge_send_cancel(int dest)

{

        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_CANCEL,

                                0, 0, dest, CANCEL_FRAME);

}

/**

 * Sends a packet to the charging station that we are

 * attempting to dock with it.

 * 

 * @param dest the charging station

 **/

void wl_recharge_send_seeking(int dest)

{

        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_SEEKING,

                                0, 0, dest, SEEKING_FRAME);

}

/**

 * Sends a packet to the charging station that

 * we have docked with it.

 *

 * @param dest the charging station we have docked with

 **/

void wl_recharge_send_docked(int dest)

{

        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_DOCKED,

                                0, 0, dest, DOCKED_FRAME);

}

/**

 * Sends a packet to the charging station telling

 * it the robot's battery life remaining.

 *

 * @param dest the charging station we have docked with

 **/

void wl_recharge_send_battery(int dest)

{

        char c[1];

        c[0]=(char)battery8();

        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_BATTERY_REQUEST,

                                c, 1, dest, 0);

}

/**

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

 **/

void wl_recharge_cleanup(void)

{

}

/**

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

 **/

void wl_recharge_timeout_handler(void)

{

        switch (recharge_state)

        {

                case POLLING:

                        if (pollCount <= 0)

                        {

                                wl_recharge_send_poll();

                                pollCount = POLL_DELAY + 1;

                        }

                        pollCount--;

                        break;

                case REQUESTING:

                        if (requestCount <= 0)

                        {

                                RECHARGE_DEBUG_PRINT("Station timed out ");

                                RECHARGE_DEBUG_PRINT("on request.\n");

                                //request failed

                                station = -1;

                                recharge_state = POLLING;

                                break;

                        }

                        requestCount--;

                        break;

                case SEEKING:

                        if (verifyCount <= 0)

                        {

                                RECHARGE_DEBUG_PRINT("Station timed out ");

                                RECHARGE_DEBUG_PRINT("on seeking verify.\n");

                                //station is down

                                station = -1;

                                recharge_state = POLLING;

                                break;

                        }

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

                                wl_recharge_send_verify(station);

                        verifyCount--;

                        break;

                case DEPARTING:

                        break;

        }

}

/**

 * 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_recharge_response_handler(int frame, int received)

{

        if (received)

                return;

        if (!received)

        {

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

                switch (frame)

                {

                        case REQUEST_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_recharge_handle_receive(char type, int source, unsigned char* packet,

                                                        int length)

{

        switch (type)

        {

                case WL_RECHARGE_POLL_STATIONS:

                        //since we aren't a station, we ignore this packet

                        break;

                case WL_RECHARGE_STATION_AVAILABLE:

                        wl_recharge_available_station(source);

                        break;

                case WL_RECHARGE_REQUEST:

                        RECHARGE_DEBUG_PRINT("Only stations should receive WL_RECHARGE_REQUEST.\n");

                        break;

                case WL_RECHARGE_REQUEST_ACCEPT:

                        wl_recharge_request_accepted(source);

                        break;

                case WL_RECHARGE_VERIFY:

                        wl_recharge_verify(source);

                        break;

                case WL_RECHARGE_CANCEL:

                        wl_recharge_cancel(source);

                        break;

                case WL_RECHARGE_SEEKING:

                        wl_recharge_seeking(source);

                        break;

                case WL_RECHARGE_DOCKED:

                        wl_recharge_docked(source);

                        break;

                //TODO: remove this, only for demo purposes

                case 9:

                        if (recharge_state == NOT_RECHARGING) {

                                recharge_i2c_set_battery_full(0);

                                wl_recharge_begin();

                                move(0,0);

                        }

                        break;

                case 10:

                        if (recharge_state == DOCKED)

                                wl_recharge_depart();

                        break;

                case WL_BATTERY_REQUEST:

                        wl_recharge_battery_request(source);

                        break;

                default:

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

                        break;

        }

}

/**

 * Called when we receive a packet alerting us that

 * a station is available for charging.

 *

 * @param source the station which has available bays

 **/

void wl_recharge_available_station(int source)

{

        if (recharge_state != POLLING)

                return;

        station = source;

        recharge_state = REQUESTING;

        requestCount = REQUEST_DELAY;

        wl_recharge_send_request(station);

}

/**

 * Called when we receive a packet saying our request

 * to charge has been accepted.

 *

 * @param source the station which has accepted our request

 **/

void wl_recharge_request_accepted(int source)

{

        if (recharge_state != REQUESTING)

        {

                RECHARGE_DEBUG_PRINT("Accepted when we weren't requesting.\n");

                return;

        }

        if (station != source)

        {

                RECHARGE_DEBUG_PRINT("Accepted by a different station.\n");

                return;

        }

        recharge_state = SEEKING;

        verifyCount = VERIFY_DELAY;

}

/**

 * Called when we receive a request to verify our status

 * in the docking procedure.

 *

 * @param source the station verifying our request

 **/

void wl_recharge_verify(int source)

{

        if (source != station)

        {

                RECHARGE_DEBUG_PRINT("Received verify from unassociated station.\n");

                wl_recharge_send_cancel(source);

                return;

        }

        switch (recharge_state)

        {

                case SEEKING:

                        wl_recharge_send_seeking(station);

                        break;

                case DOCKED:

                        wl_recharge_send_docked(station);

                        break;

                default:

                        RECHARGE_DEBUG_PRINT("Cancelled docking procedure.\n");

                        wl_recharge_send_cancel(station);

                        break;

        }

}

/**

 * Called when we receive a packet cancelling our

 * docking procedure.

 *

 * @param source the station cancelling the procedure

 **/

void wl_recharge_cancel(int source)

{

        if (station != source)

        {

                RECHARGE_DEBUG_PRINT("Received cancel from station we weren't docking with.\n");

                return;

        }

        RECHARGE_DEBUG_PRINT("Received cancellation from the charging station.\n");

        verifyCount = VERIFY_DELAY;

        switch (recharge_state)

        {

                case REQUESTING:

                        station = -1;

                        recharge_state = POLLING;

                        break;

                case SEEKING:

                        station = -1;

                        recharge_state = DEPARTING;

                        //recharge_state = POLLING;

                        break;

                case DOCKED:

                        RECHARGE_DEBUG_PRINT("Charging station kicked us out.\n");

                        wl_recharge_depart();

                        break;

                case DEPARTING:

                        //ignore, since we are already departing

                        break;

                default:

                        RECHARGE_DEBUG_PRINT("Received unexpected cancellation.\n");

                        break;

        }

}

/**

 * Called when we receive a packet stating that the

 * station believes we are seeking them.

 *

 * @param source the station we are seeking

 **/

void wl_recharge_seeking(int source)

{

        if (station != source)

        {

                RECHARGE_DEBUG_PRINT("Received seeking alert from station we aren't seeking.\n");

                wl_recharge_send_cancel(source);

                return;

        }

        verifyCount = VERIFY_DELAY;

        switch (recharge_state)

        {

                case SEEKING:

                        break;

                case DOCKED:

                case DEPARTING:

                        wl_recharge_send_docked(station);

                        break;

                default:

                        RECHARGE_DEBUG_PRINT("Received unexpected seeking confirmation.\n");

                        break;

        }

}

/**

 * Called when we receive a packet stating that

 * we are docked with a station.

 * 

 * @param source the station we are docked with

 **/

void wl_recharge_docked(int source)

{

        if (station != source)

        {

                RECHARGE_DEBUG_PRINT("Unknown station believes we are docked.\n");

                wl_recharge_send_cancel(source);

                return;

        }

        verifyCount = VERIFY_DELAY;

        switch (recharge_state)

        {

                case DOCKED:

                case DEPARTING:

                        break;

                case SEEKING:

                        //not good - we can't immediately proceed to seeking again

                        //we will leave the station and repoll it

                        RECHARGE_DEBUG_PRINT("Seeking, but station believes we are docked. ");

                        RECHARGE_DEBUG_PRINT("Leaving station and repolling.\n");

                        wl_recharge_send_cancel(source);

                        wl_recharge_begin();

                        break;

                default:

                        RECHARGE_DEBUG_PRINT("Unexpected docking verification received.\n");

                        break;

        }

}

/**

 * Called when we receive a packet requesting

 * battery level.

 * 

 * @param source the station that sent the request

 **/

void wl_recharge_battery_request(int source){

        wl_recharge_send_battery(source);

        return;

}