Colony

#include "wl_recharge_group.h"

#include <wireless.h>

#include <wl_defs.h>

#include <stdio.h>

#include <string.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_station_available(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);

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

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

#define VERIFY_FREQUENCY 4

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

{

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

{

	state = NOT_RECHARGING;

}

/**

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

 * This changes the state to DOCKED.

 **/

void wl_recharge_dock(void)

{

	if (state != SEEKING)

	{

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

		return;

	}

	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 (state != DOCKED)

	{

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

		return;

	}

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

}

/**

 * Sends a request polling for charging stations.

 **/

void wl_recharge_send_poll(void)

{

	wl_send_global_packet(RECHARGE_GROUP, WL_RECHARGE_POLL_STATIONS,

				NULL, 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(RECHARGE_GROUP, WL_RECHARGE_REQUEST,

				NULL, 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(RECHARGE_GROUP, WL_RECHARGE_VERIFY,

				NULL, 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(RECHARGE_GROUP, WL_RECHARGE_CANCEL,

				NULL, 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(RECHARGE_GROUP, WL_RECHARGE_SEEKING,

				NULL, 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(RECHARGE_GROUP, WL_RECHARGE_DOCKED,

				NULL, 0, dest, DOCKED_FRAME);

}

/**

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

	{

		case POLLING:

			if (pollCount <= 0)

			{

				wl_recharge_send_poll();

				pollCount = POLL_DELAY + 1;

			}

			pollCount--;

			break;

		case REQUESTING:

			if (requestCount <= 0)

			{

				//request failed

				station = -1;

				state = POLLING;

				break;

			}

			requestCount--;

			break;

		case SEEKING:

			if (verifyCount <= 0)

			{

				//station is down

				station = -1;

				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)

	{

		WL_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_RECHRAGE_REQUEST:

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

		default:

			WL_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 (state != POLLING)

		return;

	station = source;

	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 (state != REQUESTING)

	{

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

		return;

	}

	if (station != source)

	{

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

		return;

	}

	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)

	{

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

		wl_recharge_send_cancel(source);

		return;

	}

	switch (state)

	{

		case SEEKING:

			wl_recharge_send_seeking(station);

			break;

		case DOCKED:

			wl_recharge_send_docked(station);

			break;

		default:

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

	{

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

		return;

	}

	verifyCount = VERIFY_DELAY;

	switch (state)

	{

		case REQUESTING:

			station = -1;

			state = POLLING;

			break;

		case SEEKING:

			station = -1;

			state = POLLING;

			break;

		case DOCKED:

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

			wl_recharge_depart();

			break;

		case DEPARTING:

			//ignore, since we are already departing

			break;

		default:

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

	{

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

		wl_recharge_send_cancel(source);

		return;

	}

	verifyCount = VERIFY_DELAY;

	switch (state)

	{

		case SEEKING:

			break;

		case DOCKED:

		case DEPARTING:

			wl_recharge_send_docked(station);

			break;

		default:

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

	{

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

		wl_recharge_send_cancel(source);

		return;

	}

	verifyCount = VERIFY_DELAY;

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

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

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

			wl_recharge_send_cancel(source);

			wl_recharge_begin();

			break;

		default:

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

			break;

	}

}

#include <wl_defs.h>

#include "wl_recharge_group.h"

#include <move.h>

/**

 * Initializes recharging. Before this function may be called,

 * wl_init must be called.

 **/

void recharge_init(void)

{

	wl_recharge_register();

}

/**

 * Uninitialize recharging. Recharging and wireless must be 

 * initialized before this function may be used.

 **/

void recharge_uninit(void)

{

	wl_recharge_unregister();

}

/**

 * This function must be called frequently to perform recharging.

 * 

 * wl_do must be called often in conjunction with this function.

 * 

 * If recharge_do returns zero, then the robot is not charging, 

 * and the user has control of the robot. If this function returns

 * nonzero, then the robot is attempting to charge, and the user

 * should do nothing other than continously call this function

 * and wl_do.

 *

 * recharge_init must be called before this function

 * may be used.

 *

 * @return nonzero if the robot is recharging, zero otherwise

 **/

int recharge_do(void)

{

	switch (wl_recharge_get_state())

	{

		case NOT_RECHARGING:

			recharge_check_low_battery();

			break;

		case POLLING:

		case REQUESTING:

			//do nothing, wait for succesful request

			break;

		case SEEKING:

			recharge_seek();

			break;

		case DOCKED:

			recharge_check_charging_complete();

			break;

		case DEPARTING:

			recharge_depart();

			break;

		default:

			WL_DEBUG_PRINT("Unexpected state.\n");

			break;

	}

	return wl_recharge_get_state() != NOT_RECHARGING;

}

/**

 * Called to check if the battery is low. If the battery

 * is low and the robot needs to charge, the state

 * is changed to POLLING.

 **/

void recharge_check_low_battery()

{

	//TODO: implement

	if (battery is low)

	{

		wl_recharge_begin();

		move(0, 0);

	}

}

/**

 * Called when the robot is seeking the charging station.

 * Controls the robot's movement, and use of the BOM

 * and the homing sensor. Also checks if the robot

 * is currently charging, and changes its state

 * accordingly.

 **/

void recharge_seek()

{

	seek_station();

	if (battery is charging)

		wl_recharge_dock();

}

/**

 * Called to check if charging is complete.

 * If charging is complete, the state is

 * changed to DEPARTING.

 **/

void recharge_check_charging_complete()

{

	if (battery is charged)

		wl_recharge_depart();

}

/**

 * Called when the robot is departing from the 

 * charging station.

 **/

void recharge_depart()

{

	if (depart_station())

		wl_recharge_stop();

}

/**

 * @file departing.h Departing

 * 

 * Functions for leaving the charging station after charging

 * is complete, or if we are ejected by the station.

 **/

/**

 * @defgroup departing Departing

 *

 * @brief Leaving the charging station

 *

 * Functions for leaving the charging station.

 *

 * @{

 **/

/** @brief Leave the charging station **/

int depart_station(void);

/** @} **/

/**

 * @file wl_recharge_group.h

 * @brief A packet group for recharging messages.

 *

 * A packet group for sending and receiving recharging 

 * messages.

 *

 * @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.

 * 

 * @{

 **/

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

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

void wl_recharge_register(void);

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

void wl_recharge_unregister(void);

/**@brief Begin charging **/

void wl_recharge_begin(void);

/**@brief Stop recharging **/

void wl_recharge_stop(void);

/**@brief Robot has docked **/

void wl_recharge_dock(void);

/**@brief Robot has departed station **/

void wl_recharge_depart(void);

/**@brief Get the current robot state **/

int wl_recharge_get_state(void);

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

/**

 * @file recharge.h

 * @brief Autonomous Recharging

 *

 * Contains functions for autonomous recharging.

 *

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

 **/

/**

 * @defgroup recharge Autonomous Recharging

 * @brief Functions for autonomous recharging.

 * 

 * Functions for autonomous recharging.

 * 

 * @{

 **/

/** @brief Initialize recharging. **/

void recharge_init(void);

/** @brief Uninitialize recharging. **/

void recharge_uninit(void);

/** @brief Implements autonomous recharging functionality. **/

int recharge_do(void);

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

#include "seeking.h"

#include <homing.h>

#include <move.h>

#include <wl_token_ring.h>

#define SEEK_WITH_BOM 1

#define SEEK_WITH_HOMING 2

int state = SEEK_WITH_BOM;

void seek_station(int station)

{

	if (state == SEEK_WITH_BOM)

		state = seek_station_with_bom(station);

	else

		state = seek_station_with_homing_sensor();

}

int seek_station_with_bom(int station)

{

	int charging_loc = wl_token_get_my_sensor_reading(station);

	//move to charging station if charging_loc != -1

	//at this point, wl_csloc is set to the max bom reading

	if(charging_loc != -1)

	{

		int desiredbom = 12; // for moving backwards

		int err = desiredbom - charging_loc;

		if (err < -8)

			err += 16;

		else if (err > 8)

			err -= 16;

		int w = -err * 2;

		int v = SEEKING_BOM_VELOCITY;

		//if the error in the BOM is 'large', slow v down

		/*if( abs_int(err) > 2)

		  v = SEEKING_BOM_VELOCITY_TURN;

		*/

		move_avoid(v, w, 5);

	}

	//TODO: redesign i2c to make this cleaner and not use globals

	if(i2c_homing_sensor_data_dirty == I2C_DATA_DIRTY)

		return SEEK_WITH_BOM;

	//data is fresh, use it

	int widthcount = i2c_homing_sensor_data;

	i2c_homing_sensor_data_dirty = I2C_DATA_DIRTY;

	if(widthcount>= 4)

		return SEEK_WITH_HOMING;

	return SEEK_WITH_BOM;

}

int seek_station_with_homing_sensor(void)

{

	//if you have no new data, just continue on our current path

	if(i2c_homing_sensor_data_dirty == I2C_DATA_DIRTY)

		return SEEK_WITH_HOMING;

	//TODO: create function in homing.h to return LEFT, FORWARD, RIGHT, or NONE

	int widthcount = i2c_homing_sensor_data;

	i2c_homing_sensor_data_dirty = I2C_DATA_DIRTY;

	if (widthcount < LEFT_LOW) //can't use beacon, switch to BOM

		return SEEK_WITH_BOM;

	else if(widthcount >= CENTER_LOW)

		move(SEEKING_HOMING_VELOCITY, 0);

	else

	{

		if(widthcount >= LEFT_LOW && widthcount <= LEFT_HIGH)

			move(SEEKING_HOMING_VELOCITY, (widthcount) / 3);

		else if (widthcount >= RIGHT_LOW && widthcount <= RIGHT_HIGH)

			move(SEEKING_HOMING_VELOCITY, -(widthcount) / 3);

		else

		{

			WL_DEBUG_PRINT("Unexpected homing sensor value of ");

			WL_DEBUG_PUTI(widthcount);

			WL_DEBUG_PRINT(".\n");

		}

	}

	return SEEK_WITH_HOMING;

}

/**

 * @file seeking.h Seeking

 *

 * Functions for locating and reaching the charging station. 

 * Relies on the wireless token ring sensor matrix, as well

 * as the homing sensor.

 **/

/**

 * @defgroup seeking Seeking

 *

 * @brief Seeking the charging station

 *

 * Functions for locating and getting to the charging station.

 *

 * @{

 **/

/** @brief Seek the charging station **/

void seek_station(void);

/** @} **/

#include "departing.h"

#include <range.h>

#include <move.h>

/**

 * Leaves the charging station. The robot will

 * continue to back up until it collides with

 * a wall. range_init must be called before this

 * function may be used.

 *

 * @return nonzero if the departure is complete, 

 * zero otherwise

 **/

int depart_station(void)

{

	int front_distance = read_distance(IR2);

	if(front_distance > LEAVE_HIT_WALL_DIST_THRESH)

		seek_leave_dist_count++;

	else

		seek_leave_dist_count = 0;

	if(seek_leave_dist_count > 5)

	{

		//stop

		move(0,0);

		return 1; 

	}

	// continue moving backwards

	move(SEEK_LEAVE_STATION_VELOCITY_HIT_WALL, 0);

	return 0;

}