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)

{

	wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_BATTERY_REQUEST,

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

				wl_recharge_begin();

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

}