Colony

#include "seeking.h"

#include <move.h>

#include <serial.h>

#include "recharge_i2c.h"

#include "recharge_defs.h"

#include <wl_token_ring.h>

#define SEEK_WITH_BOM 1

#define SEEK_WITH_HOMING 2

#define SEEKING_BOM_VELOCITY -170

#define SEEKING_HOMING_VELOCITY -160

// values for homing sensor readings

#define LEFT_LOW 6

#define LEFT_HIGH 7

#define CENTER_LOW 18

#define CENTER_HIGH 19

#define RIGHT_LOW 12

#define RIGHT_HIGH 13

//function prototypes

int seek_station_with_bom(int station);

int seek_station_with_homing_sensor(void);

int seeking_state = SEEK_WITH_BOM;

void seek_station(int station)

{

        if (seeking_state == SEEK_WITH_BOM)

                seeking_state = seek_station_with_bom(station);

        else

                seeking_state = seek_station_with_homing_sensor();

}

int old_reading = 0;

int seek_station_with_bom(int station)

{

        int charging_loc = wl_token_get_my_sensor_reading(station);

        if (charging_loc != old_reading)

        {

                RECHARGE_DEBUG_PRINT("BOM reading: ");

                RECHARGE_DEBUG_PUTI(charging_loc);

                RECHARGE_DEBUG_PRINT("\n");

                old_reading = charging_loc;

        }

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

                move(v, w);

        }

        //check if we can use homing sensor

        int widthcount = recharge_i2c_get_homing_reading();

        if(widthcount>= 4)

                return SEEK_WITH_HOMING;

        return SEEK_WITH_BOM;

}

int seek_station_with_homing_sensor(void)

{

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

        int widthcount = recharge_i2c_get_homing_reading();

        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

                {

                        RECHARGE_DEBUG_PRINT("Unexpected homing sensor value of ");

                        RECHARGE_DEBUG_PUTI(widthcount);

                        RECHARGE_DEBUG_PRINT(".\n");

                }

        }

        return SEEK_WITH_HOMING;

}