Colony

#include "departing.h"

#include <rangefinder.h>

#include <move.h>

#include "recharge_defs.h"

/** We stop backing up when this is our distance from the wall

 * in our front rangefinder.

 **/

#define LEAVE_HIT_WALL_DIST_THRESH 70

/**

 * The velocity we travel at when leaving the station.

 **/

#define LEAVE_VELOCITY 200

/**

 * The counter for when we are done leaving the station.

 **/

int leave_station_count = 0;

/**

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

{

        //TODO: use rangefinders to depart

        int front_distance = range_read_distance(IR2);

        //RECHARGE_DEBUG_PUTI(front_distance);

        //RECHARGE_DEBUG_PRINT("\n");

        if(front_distance != -1 && front_distance < 160)

                leave_station_count++;

        else

                leave_station_count = 0;

        if(leave_station_count > 1000)

        {

                //stop

                move(0,0);

                leave_station_count = 0;

                return 1; 

        }

        // continue moving backwards

        move(LEAVE_VELOCITY, 0);

        return 0;

}