Colony

#include "wl_charging_station.h"

#include <wireless.h>

#include <wl_defs.h>

#include <stdio.h>

#include <string.h>

#include "charging_defs.h"

#include "charging.h"

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

// the handler

PacketGroupHandler wl_charging_handler =

		{WL_RECHARGE_GROUP, wl_charging_timeout_handler,

		wl_charging_response_handler, wl_charging_handle_receive,

		wl_charging_cleanup};

//timing

//timer ticks between polls

#define VERIFY_DELAY 8

#define VERIFY_FREQUENCY 4

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

/**

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

}

/**

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

		{

			charging_cancel(robot);

			continue;

		}

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

			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;

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

}

/**

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

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

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

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

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

}

/**

 * @file charging_defs.h

 * @brief Definitions for charging

 *

 * Contains definitions convenient for use in the charging station.

 *

 * @author Brian Coltin, Colony Project, CMU Robotics Club

 **/

//comment this out to not output debugging information

#define STATION_DEBUG

#ifdef STATION_DEBUG

#define STATION_DEBUG_PRINT( s ) usb_puts( s )

#else

#define STATION_DEBUG_PRINT( s )

#endif

/**

 * @defgroup defs Charging Station Definitions

 * @brief Defines used in the charging station

 *

 * Definitions used in the charging station.

 *

 * @{

 **/

/** @} **/

/**

 * @file wl_charging_station.h

 * @brief A packet group for recharging messages

 *

 * A packet group for sending and receiving recharging 

 * messages, implemented from the perspective of a charging

 * station.

 *

 * @author Brian Coltin, Colony Project, CMU Robotics Club

 **/

/**

 * @defgroup wlrecharge Recharging Packets

 * @brief Functions for sending and receiving recharging packets

 * 

 * Functions for sending and receiving recharging packets.

 * 

 * @{

 **/

/**@brief Register this packet group with the wireless library **/

void wl_station_register(void);

/**@brief Unregister this packet group with the wireless library **/

void wl_station_unregister(void);

/** @} **/ // end defgroup

/*int assign_bay( int robot ) // 'smart' assign_bay()

}*/

#include "charging.h"

#include "charging_defs.h"

#include <stdlib.h>

#include <serial.h>

#include <wl_token_ring.h>

#include "wl_charging_station.h"

//set this to the number of bays

#define NUM_BAYS 2

//Function prototypes

void charging_bom_on(void);

void charging_bom_off(void);

int charging_bom_read(void);

//Structures

typedef struct

{

	int state;

	int bay;

	int verifyCount;

} ChargingRobot;

ChargingRobot** robots;

int robotsSize = 0;

int numRobots = 0;

int iteratorPos = 0;

int bays[NUM_BAYS];

int is_seeking = 0;

/**

 * Call this function to initialize charging.

 * wl_token_ring_register must be called before

 * this function.

 **/

void charging_init(void)

{

	robots = malloc(robotsSize * sizeof(ChargingRobot*));

	if (!robots)

	{

		STATION_DEBUG_PRINT("Out of memory.\n");

		return;

	}

	int i;

	for (i = 0; i < robotsSize; i++)

		robots[i] = NULL;

	for (i = 0; i < NUM_BAYS; i++)

		bays[i] = -1;

	// call our own special function to set the BOM

	wl_token_ring_set_bom_functions(charging_bom_on,

		charging_bom_off, charging_bom_read);

	wl_station_register();

}

/**

 * Initializes the iterator through the robots

 * which are using the charging station.

 **/

void charging_robot_iterator_init(void)

{

	iteratorPos = 0;

}

/**

 * Returns zero if the iterator has gone through

 * all robots that are using the charging station

 * since the last call to charging_robot_iterator_init,

 * nonzero otherwise.

 *

 * @return if the iterator has gone through all the robots

 **/

int charging_robot_iterator_has_next(void)

{

	while (iteratorPos < robotsSize)

	{

		if (robots[iteratorPos] != NULL)

			return 1;

		iteratorPos++;

	}

	return 0;

}

/**

 * Returns the id of the next robot in the iterator

 * of robots that are using the charging station.

 *

 * @return the id of a robot using the station

 **/

int charging_robot_iterator_next(void)

{

	while (iteratorPos < robotsSize)

	{

		if (robots[iteratorPos] != NULL)

		{

			iteratorPos++;

			return iteratorPos - 1;

		}

	}

	return -1;

}

/**

 * Return the verification count for a robot

 * using the station. If the count is zero,

 * then the robot is probably dead.

 *

 * @param robot the id of the robot to check

 * 

 * @return the verify count for robot robot

 **/

int charging_get_verify_count(int robot)

{

	if (robot >= robotsSize || robot < 0)

	{

		STATION_DEBUG_PRINT("Index out of range.\n");

		return -1;

	}

	if (robots[robot] == NULL)

	{

		STATION_DEBUG_PRINT("Robot not known to charging station.\n");

		return -1;

	}

	return robots[robot]->verifyCount;

}

/**

 * Sets the verify count for the specified robot.

 *

 * @param robot the robot to specify the count for

 * @param count the new verify count

 **/

void charging_set_verify_count(int robot, int count)

{

	if (robot >= robotsSize || robot < 0)

	{

		STATION_DEBUG_PRINT("Index out of range.\n");

		return;

	}

	if (robots[robot] == NULL)

	{

		STATION_DEBUG_PRINT("Robot not known to charging station.\n");

		return;

	}

	robots[robot]->verifyCount = count;

}

/**

 * Called when charging has been cancelled for a 

 * specific robot.

 *

 * @param robot the robot to cancel charging for

 **/

void charging_cancel(int robot)

{

	if (robot < 0 || robot >= robotsSize || robots[robot] == NULL)

	{

		STATION_DEBUG_PRINT("Attempted to cancel charging for an ");

		STATION_DEBUG_PRINT("unknown robot.\n");

		return;

	}

	//allow other robots to seek

	if (robots[robot]->state == STATE_SEEKING)

	{

		//TODO : deactivate BOMs and homing beacons

		is_seeking = 0;

	}

	//free the bay

	bays[robots[robot]->bay] = -1;

	free(robots[robot]);

	robots[robot] = NULL;

	numRobots--;

}

/**

 * Assigns a bay to a specific robot.

 *

 * @param robot the robot to assign the bay to

 *

 * @return the identifier for the assigned bay

 **/

int assign_bay(int robot)

{

	int bay = 0;

	//TODO: implement

	bays[bay] = robot;

	return bay;

}

/**

 * Called when a robot begins seeking.

 *

 * @param robot the robot which has begun seeking

 **/

void charging_begin_seeking(int robot)

{

	if (robot > robotsSize)

	{

		int nextSize = robot + 1;

		ChargingRobot** temp = malloc(nextSize * sizeof(ChargingRobot*));

		if (temp == NULL)

		{

			STATION_DEBUG_PRINT("Out of memory.\n");

			return;

		}

		int i;

		for (i = 0; i < robotsSize; i++)

			temp[i] = robots[i];

		for (; i < nextSize; i++)

			temp[i] = NULL;

		free(robots);

		robots = temp;

		robotsSize = nextSize;

	}

	if (charging_is_robot_seeking())

	{

		STATION_DEBUG_PRINT("A robot is already seeking.\n");

		return;

	}

	//check if robot has reverted from docking to seeking

	if (robots[robot] != NULL)

	{

		switch (robots[robot]->state)

		{

			case STATE_SEEKING:

				//do nothing

				STATION_DEBUG_PRINT("Robot is already ");

				STATION_DEBUG_PRINT("seeking.\n");

				break;

			case STATE_DOCKED:

				robots[robot]->state = STATE_SEEKING;

				is_seeking = 1;

				//TODO: active robots[robot]->bay

				break;

			default:

				STATION_DEBUG_PRINT("Robot is in an ");

				STATION_DEBUG_PRINT("unexpected state.\n");

				break;

		}

		return;

	}

	if (!charging_is_space_available())

	{

		STATION_DEBUG_PRINT("No bays are available for charging.\n");

		return;

	}

	ChargingRobot* r = malloc(sizeof(ChargingRobot));

	if (!r)

	{

		STATION_DEBUG_PRINT("Out of memory.\n");

		return;

	}

	r->state = STATE_SEEKING;

	r->bay = assign_bay(robot);

	r->verifyCount = 0;

	robots[robot] = r;

	numRobots++;

	is_seeking = 1;

	//TODO: activate r->bay

}

/**

 * Called when a robot has docked.

 *

 * @param robot the robot which has docked

 **/

void charging_dock(int robot)

{

	if (robots[robot] == NULL)

	{

		STATION_DEBUG_PRINT("Should not proceed from not ");

		STATION_DEBUG_PRINT("charging to docked.\n");

		return;

	}

	if (robots[robot]->state == STATE_DOCKED)

	{

		STATION_DEBUG_PRINT("Robot is already docked.\n");

		return;

	}

	//robot was seeking before

	is_seeking = 0;

	//TODO: deactivate boms and homing beacons

	robots[robot]->state = STATE_DOCKED;

}

/**

 * Checks if there are any available bays

 * for a robot to occupy.

 *

 * @return nonzero if there are bays available,

 * and zero otherwise.

 **/

int charging_is_space_available(void)

{

	return numRobots < NUM_BAYS;

}

/**

 * Checks if there is a robot which

 * is currently seeking the station.

 *

 * @return nonzero if a robot is seeking the

 * station, zero otherwise.

 **/

int charging_is_robot_seeking(void)

{

	return is_seeking;

}

/**

 * Returns the state of the specified robot.

 *

 * @param robot the robot to get the state of

 *

 * @return either STATE_NOT_CHARGING, STATE_SEEKING, or STATE_DOCKED

 **/

int charging_get_robot_state(int robot)

{

	if (robot >= robotsSize || robots[robot] == NULL)

		return STATE_NOT_CHARGING;

	return robots[robot]->state;

}

/**

 * Called when the BOM needs to be turned on for the 

 * token ring. If a robot is seeking, only the BOM of

 * the bay that is being sought will turn on. If no

 * robot is seeking, the BOM for all of the bays will

 * turn on.

 **/

void charging_bom_on(void)

{

	//TODO: implement

}

/**

 * Called when the BOM needs to be turned off for the

 * token ring. Turns off the BOM for all bays.

 **/

void charging_bom_off(void)

{

	//TODO: implement

}

/**

 * The charging station BOM can only emit, so

 * we just return -1.

 **/

int charging_bom_read(void)

{

	return -1;

}

#include <firefly+_lib.h>

#include "charging.h"

int main(void)

{

	analog_init();

	led_init();

	orb_init();

	buzzer_init();

	usb_init(BAUD115200);

	xbee_init(BAUD9600);

	//dragonfly_init(ALL_ON);

	wl_init();

	wl_token_ring_register();

	charging_init();

	while (1)

	return 0;

/**

 * @file charging.h Charging

 *

 * @brief Charging station functions

 *

 * Contains functions for the charging station, controlling

 * bay management, homing beacons and BOMS.

 **/

/**

 * @defgroup charging Charging Station

 * 

 * @brief Charging Station Functionality

 *

 * Contains functions for the charging station,

 * controlling bay management, the homing beacons

 * and BOMS.

 *

 * @{

 **/

//possible robot states

#define STATE_NOT_CHARGING 1

#define STATE_SEEKING 2

#define STATE_DOCKED 3

/** @brief Initialize charging **/

void charging_init(void);

/** @brief Initialize an iterator through the robots **/

void charging_robot_iterator_init(void);

/** @brief Check if there is another robot to iterate through **/

int charging_robot_iterator_has_next(void);

/** @brief Get the next robot's id **/

int charging_robot_iterator_next(void);

/** @brief Get the verify count for a robot **/

int charging_get_verify_count(int robot);

/** @brief Set the verify count for a robot **/

void charging_set_verify_count(int robot, int count);

/** @brief Cancel charging for a robot **/

void charging_cancel(int robot);

/** @brief Begin seeking for a robot **/

void charging_begin_seeking(int robot);

/** @brief Dock a robot with the charging station **/

void charging_dock(int robot);

/** @brief Check if room is available to charge **/

int charging_is_space_available(void);

/** @brief Check if a robot is currently seeking **/

int charging_is_robot_seeking(void);

/** @brief Get the supposed state of a robot **/

int charging_get_robot_state(int robot);

/** @} **/