Colony

#include <dragonfly_lib.h>

#include "smart_run_around_fsm.h"

/*A simple behavior for navigating in an environment, i.e. avoiding walls and getting stuck.

Could be better at not getting stuck.

Latest revision only has two accessible states: move and reverse.

*/

/* private function */

void run_around_evaluate_state(void);

void run_around_init(void)

{

  //range_init();

  //analog_init(1);

  //motors_init();

  //orb_init();

  //orb_enable();

  usb_init();

  /*Start in the default state, MOVING*/ 

  avoid_state=MOVING;

  /*Set timers to their maximum values.*/

  crazy_count=CRAZY_MAX;

  backup_count=0; 

  pControl=0;

  /*Initialize distances to zero.*/ 

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

  //orb_set_color(GREEN);

}

/*The main function, call this to update states as frequently as possible.*/

void run_around_FSM(void) {

  /*Default to moving.*/ 

  avoid_state=MOVING;

  /*The following lines ensure that undefined (-1) values

  will not update the distances.*/ 

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

  /*If the crazy count is in it's >>3 range, it acts crazy.*/

  if(crazy_count<=(CRAZY_MAX>>3))

  {

    avoid_state=CRAZY;

    crazy_count--;

    if(crazy_count<0) crazy_count=CRAZY_MAX;

    run_around_evaluate_state();

    return;

  }

  //Checks the forward distance to see if it should back up, if so...state backwards.

  if((d2!=-1)&&(d2 < 150)){

      backup_count=BACKUP_MAX;

      avoid_state=BACKWARDS;

      run_around_evaluate_state();

      return;

  }

  /*

  if(d1 < 120 || d3 < 120) {

                avoid_state = BACKWARDS;

                backup_count = BACKUP_MAX;

                run_around_evaluate_state();

                return;

  }

  */

  if(backup_count<BACKUP_MAX){

    avoid_state=BACKWARDS; 

    if(backup_count<0)

      backup_count=BACKUP_MAX;

    run_around_evaluate_state();

    return;

  }

  /*Should evaluate an expression from -255 to 255 to pass to move.*/

  pControl= ((d3-d1) + (d4-d5)) >> TURN_CONSTANT;

  if(pControl>PCONTROL_CRAZY_LIMIT || pControl<-PCONTROL_CRAZY_LIMIT) crazy_count--;

  /*i.e. if you really want to turn for an extended period of time...you're probably stuck.*/

  /*Debug stuff:*/

  /*usb_puts("pControl evaluating: ");

  usb_puti(pControl);

  usb_puts("\n\r");

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

  run_around_evaluate_state();

}

//Acts on state change.

void run_around_evaluate_state(){

    switch(avoid_state){

    case(MOVING): //orb_set_color(GREEN);

      move(STRAIT_SPEED,-pControl);

      break;

    case(BACKWARDS): //orb_set_color(ORANGE);

      move(-STRAIT_SPEED,0);

      break;

    case(CRAZY): //orb_set_color(RED);

      /*TODO: Implement a crazy state.*/

      move(STRAIT_SPEED,-pControl);

      break;

    default:

      /*Should never get here, go strait.*/

      move(100,0); //orb_set_color(BLUE);

      break;

  }

}