Colony

#include "seeking.h"

#include <move.h>

#include <serial.h>

#include "recharge_i2c.h"

#include "recharge_defs.h"

#include "homing.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 -170

#define SEEKING_HOMING_FORWARD_VELOCITY -180

//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 * 4; // was *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)

{

        int widthcount = recharge_i2c_get_homing_reading();

        int direction = homing_direction(widthcount);

        switch(direction)

        {

                case HOMING_NONE:

                        return SEEK_WITH_BOM;

                        break;

                case HOMING_FORWARD:

                        move(SEEKING_HOMING_FORWARD_VELOCITY, 0);

                        break;

                case HOMING_LEFT:

                        move(SEEKING_HOMING_VELOCITY, 3);

                        break;

                case HOMING_RIGHT:

                        move(SEEKING_HOMING_VELOCITY, -3);

                        break;

        }

        return SEEK_WITH_HOMING;

}