Colony

#include "homing.h"

#include "recharge_defs.h"

#include <serial.h>

// values for homing sensor readings

#define LEFT_LOW    6

#define LEFT_HIGH   8

#define CENTER_LOW  17

#define CENTER_HIGH 19

#define RIGHT_LOW   11

#define RIGHT_HIGH  13

/**

 * translates homing sensor data into the

 * direction the robot needs to turn

 * (or if it needs to use the BOM).

 **/

int homing_direction(int beaconcount)

{

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

                return HOMING_NONE;

        else if(beaconcount >= CENTER_LOW)

                return HOMING_FORWARD;

        else

        {

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

                        return HOMING_LEFT;

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

                        return HOMING_RIGHT;

                else

                {

                        RECHARGE_DEBUG_PRINT("Unexpected homing sensor value of ");

                        RECHARGE_DEBUG_PUTI(beaconcount);

                        RECHARGE_DEBUG_PRINT(".\n");

                }

        }

    return HOMING_NONE;

}