Colony

#include <dragonfly_lib.h>

#include <wl_defs.h>

#include <wireless.h>

#include <wl_token_ring.h>

#include "smart_run_around_fsm.h"

#include "lemmings.h"

/*A simple behavior for following the leader.

  SINGLE_FILE pattern not implemented yet

*/

//The States:

#define LEAD         1        // move around randomly, leading a chain

#define FOLLOW         2        // follow another bot

#define FOLLOW_MULTI_DEFAULT 1 // set default multi-following pattern

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

/* Globals */

int cur_state;                 // current state

int crazy_count;    // Counter for a random behavior

int follow_bot;                // follow this bot (-1 if leader)

int follow_direction; // follow in this direction

int follow_multi;                // set to 0 for single-file following, 1 for tree following

/* Prototypes */

static void lemmings_set_pattern(int pattern);

static void lemmings_Prims(void);

static int lemmings_get_edge_weight(int robot1, int robot2);

static void lemmings_evaluate_state(void);

void lemmings_init() {

//  dragonfly_init(ALL_ON);

//  analog_init();

//  motors_init();

//  range_init();

//  orb_init();

//  orb_enable();

//  usb_init();

  run_around_init(); // prepare for moving

  /*Start in the default state, LEAD*/

  cur_state = LEAD;

  /*Set timers to their maximum values.*/

  crazy_count = CRAZY_MAX;

  // set to default state

  follow_bot = -1;

  cur_state = LEAD;

  usb_puts("init_following: robot #");

  usb_puti(follow_bot);

  // set pattern

  lemmings_set_pattern(FOLLOW_MULTI_DEFAULT);

}

// set the following pattern

// SINGLE_FILE if pattern <= 0

// TREE        if pattern >= 1

static void lemmings_set_pattern(int pattern) {

  if (pattern <= 0) {

    follow_multi = 0;

  } else {

    follow_multi = 1;

  }

  orb_set_color(ORANGE);

}

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

void lemmings_FSM(void) {

  int bom = -1;

  int i = 0;

  int old_follow_bot;

  orb_set_color(WHITE);

  if (follow_bot < 0) {

    lemmings_Prims(); // if we are not following, then attempt to follow

  }

  // if we are following a bot, make sure we can still see it

  old_follow_bot = follow_bot;

  for (i=0;follow_bot>=0&&i<10;i++) {

    bom = wl_token_get_sensor_reading(wl_get_xbee_id(),follow_bot);

    if (bom < 0) {

      // get new bot to follow

      lemmings_Prims();

      if (old_follow_bot == follow_bot)

        break; // some sort of error

      continue;

    } else {

      cur_state = FOLLOW; // set to follow state

      switch(bom) {

      case 0:

      case 15:

      case 14:

      case 13:

      case 12:

        follow_direction = 255;

        break;

      case 1:

        follow_direction = 150;

        break;

      case 2:

        follow_direction = 100;

        break;

      case 3:

        follow_direction = 50;

        break;

      case 4:

        follow_direction = 0;

        break;

      case 5:

        follow_direction = -50;

        break;

      case 6:

        follow_direction = -100;

        break;

      case 7:

        follow_direction = -150;

        break;

      case 8:

      case 9:

      case 10:

      case 11:

        follow_direction = -255;

        break;

      default:

        follow_direction = 0;

        break;

      }

      break; // break from loop

    }

  }

  /*If the crazy count is in it's >>3 range, and we are currently a follower, be a leader.*/

  /* CURRENTLY DISABLED

  if(cur_state == FOLLOW && crazy_count<=(CRAZY_MAX>>3)) {

    cur_state = LEAD;

  }*/

  usb_puts("following: ");

  usb_puti(follow_bot);

  usb_puts("bom: ");

  usb_puti(bom);

  usb_puts("\n\r");

  lemmings_evaluate_state(); // evaluate state

}

// create connected components and pick a leader for each chain

// use modified Prim's alogrithm to find local spanning tree

// CURRENTLY JUST LOOKS FOR CLOSEST ROBOT TO FOLLOW

static void lemmings_Prims(void) {

  int nodeA = wl_get_xbee_id(); // get id of current robot

  int nodeB = -1;

  int cur_weight = -1;

  int best_weight = 1000; // set to infinity initially

  int best_node = -1;

  int i = 0;

  orb_set_color(BLUE);

  // iterate through token ring

  wl_token_iterator_begin();

  for(i=0; wl_token_iterator_has_next() && i<10; i++) {

    nodeB = wl_token_iterator_next();

    usb_puts("botB: ");

    usb_puti(nodeB);

    usb_puts("\n\r");

    if (nodeB < 0) {

      break; // no more bots

    } else if (nodeB == nodeA) {

      continue; // can't follow self

    }

    cur_weight = lemmings_get_edge_weight(nodeA,nodeB);

    usb_puts("weight: ");

    usb_puti(cur_weight);

    usb_puts("\n\r");

    if (cur_weight >= 0 && cur_weight < best_weight) {

      // this is new best node, so save values

      best_weight = cur_weight;

      best_node = nodeB;

    }

  }

  if (best_node == -1 || (best_weight > 5 && nodeA < nodeB)) {

    // no robots to follow, or choosing to be a leader

    cur_state = LEAD;

    follow_bot = -1;

  } else {

    // follow bot

    cur_state = FOLLOW;

    follow_bot = nodeB;

  }

}

// get edge weight of sensor matrix

// add in BOM range data when BOM 1.5 comes out

static int lemmings_get_edge_weight(int robot1, int robot2) {

  int weight = -1;

  int bom = wl_token_get_sensor_reading(robot1,robot2);

  // since we only have positioning data, give better weight to

  // bots in from of us

  switch(bom) {

  case 12:

    weight++;

  case 13:

  case 11:

    weight++;

  case 14:

  case 10:

    weight++;

  case 15:

  case 9:

    weight++;

  case 0:

  case 8:

    weight++;

  case 1:

  case 7:

    weight++;

  case 2:

  case 6:

    weight++;

  case 3:

  case 5:

    weight++;

  case 4:

    break;

  default:

    weight = -1;

    break;

  }

  return weight;

}

//Acts on state change.

static void lemmings_evaluate_state() {

  usb_puts("state: ");

  usb_puti(cur_state);

  usb_puts("\n\r");

  switch(cur_state){

  case(LEAD):

    //move_avoid(200,0,50);

    //orb_set_color(GREEN);

    run_around_FSM(); // do random run around

    break;

  case(FOLLOW):

    move(200,follow_direction); // move in direction of bot

    orb_set_color(RED);

    break;

  default:

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

          move(200,0);

          orb_set_color(BLUE);

          break;

        }

}