Colony

#include <dragonfly_lib.h>

#include "smart_run_around_fsm.h"

void run_around_init(void)

{

  range_init();

  orb_init();

  state = FORWARD;  // start out moving forward

  // initialize rangefinder values to 0

  d1 = 0, d2 = 0, d3 = 0, d4 = 0, d5 = 0;

}

/* evaluate rangefinder input and update state

 * call this function as often as possible to avoid collisions

 */

void run_around_FSM(void)

{

    /* TODO: find a better way to handle rangefinder input

     * robot should deal with -1s (obstacles too close or too far to detect)

     * in a way that keeps it from crashing or driving in circles

     */

    // do not update distances when rangefinders return -1

    // otherwise update distances with new rangefinder values

    int16_t temp;

    temp = range_read_distance(IR1);

    d1 = (temp == -1) ? d1 : temp;

    temp = range_read_distance(IR2);

    d2 = (temp == -1) ? d2 : temp;

    temp = range_read_distance(IR3);

    d3 = (temp == -1) ? d3 : temp;

    temp = range_read_distance(IR4);

    d4 = (temp == -1) ? d4 : temp;

    temp = range_read_distance(IR5);

    d5 = (temp == -1) ? d5 : temp;

    // TODO: double-check rangefinder numbers and positions

    // update state based on rangefinder input

    if (d2 < IR2_DANGER)    // avoid frontal collision by turning in place

        state = SPIN;

    /* nowhere to turn, so don't

     * will probably need to turn around soon, but not yet

     */

    else if (d1 < IR13_DANGER && d3 < IR13_DANGER)

        state = FORWARD;

    else if (d1 < IR13_DANGER)        // avoid right-side collision by turning left

        state = LEFT;

    else if (d3 < IR13_DANGER)        // avoid left-side collision by turn right

        state = RIGHT;

    else if (d2 < IR2_INTEREST)        // should turn to avoid obstacle up ahead

    {

        if (d3 >= d1)        // more room on right

            state = RIGHT;

        else        // more room on left

            state = LEFT;

    }

    else    // no obstacles close by, so keep going straight

        state = FORWARD;

    /* Debugging via USB output */

    usb_puts("IR1: ");

    usb_puti(d1);

    usb_puts(" IR2: ");

    usb_puti(d2);

    usb_puts(" IR3: ");

    usb_puti(d3);

    usb_puts(" IR4: ");

    usb_puti(d4);

    usb_puts(" IR5: ");

    usb_puti(d5);

    usb_puts("\n\r");

    evaluate_state();        // take action on updated state

}

// behave according to current state

// TODO: double-check orb numbers

// TODO: adjust speeds after testing

void evaluate_state(void)

{

    switch (state)

    {

        case FORWARD:

            orb_set_color(GREEN);

            move(HALF_SPD, 0);        // drive straight forward

            break;

        case LEFT:

            orbs_set_color(YELLOW, GREEN);  // green on left side

            move(HALF_SPD, NRML_TURN);        // drive forward and to the left

            break;

        case RIGHT:

            orbs_set_color(GREEN, YELLOW);  // green on right side

            move(HALF_SPD, -NRML_TURN);        // drive forward and to the right

            break;

        case SPIN:

            orb_set_color(RED);

            move(0, NRML_TURN);        // spin CCW without traveling

            break;

        default:    // should never reach this

            orb_set_color(PURPLE);

            move(0, 0);        // stop completely

    }

}