Colony

#include <move.h>

#include <battery.h>

#include <dio.h>

#include <lights.h>

#include <motor.h>

#include <serial.h>

#include <wl_defs.h>

#include "wl_recharge_group.h"

#include "seeking.h"

#include "departing.h"

#include "recharge_i2c.h"

#include "recharge_defs.h"

// Function prototypes

void recharge_check_low_battery(void);

void recharge_check_charging_complete(void);

void recharge_seek(void);

void recharge_depart(void);

//Global variables

#ifdef RECHARGE_DEBUG

int old_state = -1;

#endif

/**

 * Initializes recharging. Before this function may be called,

 * wl_init must be called.

 **/

void recharge_init(void)

{

        wl_recharge_register();

        //TODO: uncomment to use homing sensor

        recharge_i2c_init();

}

/**

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

        }

        // show colors if we are debugging

        #ifdef RECHARGE_DEBUG

        if (old_state != wl_recharge_get_state())

        {

                old_state = wl_recharge_get_state();

                switch (old_state)

                {

                        case NOT_RECHARGING:

                                RECHARGE_DEBUG_PRINT("Not recharging.\n");

                                orb_set_color(GREEN);

                                break;

                        case POLLING:

                                RECHARGE_DEBUG_PRINT("Polling available stations.\n");

                                orb_set_color(CYAN);

                                break;

                        case REQUESTING:

                                RECHARGE_DEBUG_PRINT("Requesting a station.\n");

                                orb_set_color(RED);

                                break;

                        case SEEKING:

                                RECHARGE_DEBUG_PRINT("Seeking.\n");

                                orb_set_color(YELLOW);

                                break;

                        case DOCKED:

                                RECHARGE_DEBUG_PRINT("Docked.\n");

                                orb_set_color(BLUE);

                                break;

                        case DEPARTING:

                                RECHARGE_DEBUG_PRINT("Departing.\n");

                                orb_set_color(PURPLE);

                                break;

                        default:

                                WL_DEBUG_PRINT("Unexpected state.\n");

                                break;

                }

        }

        #endif

        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: use battery_low instead of button press

        //if (battery_low())

        if (button2_click())

        {

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

        //TODO: check if battery is charging

        //if (battery is charging)

        if (button2_click())

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

{

        //TODO: implement

        //if (battery is charged)

        if (button2_click())

        {

                motors_off();

                wl_recharge_depart();

        }

}

/**

 * Called when the robot is departing from the 

 * charging station.

 **/

void recharge_depart()

{

        if (depart_station())

                wl_recharge_stop();

}