Colony

#include "seeking.h"

#include <homing.h>

#include <move.h>

#include <wl_token_ring.h>

#define SEEK_WITH_BOM 1

#define SEEK_WITH_HOMING 2

int state = SEEK_WITH_BOM;

void seek_station(int station)

{

        if (state == SEEK_WITH_BOM)

                state = seek_station_with_bom(station);

        else

                state = seek_station_with_homing_sensor();

}

int seek_station_with_bom(int station)

{

        int charging_loc = wl_token_get_my_sensor_reading(station);

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

                //if the error in the BOM is 'large', slow v down

                /*if( abs_int(err) > 2)

                  v = SEEKING_BOM_VELOCITY_TURN;

                */

                move_avoid(v, w, 5);

        }

        //TODO: redesign i2c to make this cleaner and not use globals

        if(i2c_homing_sensor_data_dirty == I2C_DATA_DIRTY)

                return SEEK_WITH_BOM;

        //data is fresh, use it

        int widthcount = i2c_homing_sensor_data;

        i2c_homing_sensor_data_dirty = I2C_DATA_DIRTY;

        if(widthcount>= 4)

                return SEEK_WITH_HOMING;

        return SEEK_WITH_BOM;

}

int seek_station_with_homing_sensor(void)

{

        //if you have no new data, just continue on our current path

        if(i2c_homing_sensor_data_dirty == I2C_DATA_DIRTY)

                return SEEK_WITH_HOMING;

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

        int widthcount = i2c_homing_sensor_data;

        i2c_homing_sensor_data_dirty = I2C_DATA_DIRTY;

        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

                {

                        WL_DEBUG_PRINT("Unexpected homing sensor value of ");

                        WL_DEBUG_PUTI(widthcount);

                        WL_DEBUG_PRINT(".\n");

                }

        }

        return SEEK_WITH_HOMING;

}