Colony

/**

 */

#include <string.h>

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

*/

// FSM states

#define MOVING 12           //Move strait.

#define BACKWARDS 15        //Move backwards. (Front close to wall.)

#define STOP 16             //Stop.  The default state, (Something broke).

#define CRAZY 40            //Erratic behavior that occurs more often when the robot is frequently trying to turn. (i.e. may be stuck.)

#define LEFT 37             //Left

#define RIGHT 39            //Right

#define BACKUP_MAX 20

#define CRAZY_MAX 200       //The number of counts between "crazy moments"

#define STRAIT_SPEED 185    //The speed when going strait or backing up.

#define TURN_CONSTANT 2

#define PCONTROL_CRAZY_LIMIT 80

// Globals

int avoid_state;    /*State machine variable.*/

int crazy_count;    /*Counter for a 'get unstuck' behavior.*/

int backup_count;        /*Counter for backup duration.*/

int pControl;                /*Proportional control variable, determines turn direction.*/

#define NUM_IR_SENSORS 5

int dist[NUM_IR_SENSORS];

static void evaluate_state(void);

void run_around_init(void) {

/*   range_init(); */

/*   analog_init(); */

/*   motors_init(); */

/*   orb_init(); */

/*   orb_enable(); */

/*   usb_init(); */

  avoid_state = MOVING;

  /*Set timers to their maximum values.*/

  crazy_count = CRAZY_MAX;

  backup_count = 0;

  pControl = 0;

  /*Initialize distances to zero.*/

  memset(dist, 0, NUM_IR_SENSORS * sizeof(int));

}

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

  dist[0] = (temp == -1) ? dist[0] : temp;

  temp=range_read_distance(IR2);

  dist[1] = (temp == -1) ? dist[1] : temp;

  temp=range_read_distance(IR3);

  dist[2] = (temp == -1) ? dist[2] : temp;

  temp=range_read_distance(IR4);

  dist[3] = (temp == -1) ? dist[3] : temp;

  temp=range_read_distance(IR5);

  dist[4] = (temp == -1) ? dist[4] : 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 ((dist[1] != -1) && (dist[1] < 150)){

    backup_count=BACKUP_MAX;

    avoid_state=BACKWARDS;

    evaluate_state();

    return;

  }

  /*

  if(dist[0] < 120 || dist[2] < 120) {

                avoid_state = BACKWARDS;

                backup_count = BACKUP_MAX;

                evaluate_state();

                return;

  }

  */

  if(backup_count<BACKUP_MAX){

    avoid_state=BACKWARDS;

    if (backup_count<0) {

      backup_count = BACKUP_MAX;

    }

    evaluate_state();

    return;

  }

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

  pControl= ((dist[2]-dist[0]) + (dist[3]-dist[4])) >> 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(dist[0]);

  usb_puts(" IR2: ");

  usb_puti(dist[1]);

  usb_puts(" IR3: ");

  usb_puti(dist[2]);

  usb_puts(" IR4: ");

  usb_puti(dist[3]);

  usb_puts(" IR5: ");

  usb_puti(dist[4]);

  usb_puts("\n\r");*/

  evaluate_state();

}

//Acts on state change.

static void 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;

  }

}