Colony

#include "dragonfly_lib.h"

#include "smart_run_around_fsm.h"

#include <wireless.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.

*/

void run_around_init(void)

{

  range_init();

  /*Start in the default state, MOVING*/

  avoid_state=MOVING;

  prev_state = avoid_state;

  /*Set timers to their maximum values.*/

  crazy_count=CRAZY_MAX;

  backup_count=BACKUP_MAX;

  pControl=0;

  /*Initialize distances to zero.*/

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

  orb_set_color(BLUE);

}

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

void run_around_FSM(void) {

  prev_state = avoid_state;

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

    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;

      evaluate_state();

      return;

  }

  if(backup_count<BACKUP_MAX){

    usb_puts("DO I EVER GET HERE?\n\r");

    avoid_state=BACKWARDS;

    if(backup_count<0)

      backup_count=BACKUP_MAX;

    evaluate_state();

    return;

  }

  //usb_puts("avoid_state set to MOVING\n\r");

  avoid_state = MOVING;

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

  evaluate_state();

}

//Acts on state change.

void evaluate_state(){

    switch(avoid_state){

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

      if(prev_state == BACKWARDS) {

        delay_motors();

      }

      move(STRAIT_SPEED,-pControl);

      //motor1_set(FORWARD,200);

      //motor2_set(FORWARD,200);

      break;

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

      if(prev_state == MOVING) {

        delay_motors();

      }

      move(-STRAIT_SPEED-50,0);

      //motor1_set(BACKWARD,200);

      //motor2_set(BACKWARD,200);

      break;

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

      /*TODO: Implement a crazy state.*/

      if(prev_state == BACKWARDS) {

        delay_motors();

      }

      move(STRAIT_SPEED,-pControl);

      break;

    default:

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

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

      break;

  }

}

void delay_motors() {

  int count = 0;

  move(0,0);

  //usb_puts("called delay_motors()\n\r");

  while(count < 300) {

    delay_ms(1);

    wl_do();

    count++;

  }

}