Colony

#include <dragonfly_lib.h>

#include <wl_defs.h>

#include <wireless.h>

#include <wl_token_ring.h>

//#include <colonet_dragonfly.h>

//#include "smart_run_around_fsm.h"

#include "lemmings.h"

/*A simple behavior for following the leader

  SINGLE_FILE pattern not implemented yet

*/

// FSM states

#define LEAD 0    // be a leader

#define FOLLOW 1        // follow another robot

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

#define SPEED 200 // set default speed

/* Globals */

static int cur_state;                 // current state

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

static int global_leader; // this is the global leader

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

static int local_id;      // this is the local robot id

/* Internal prototypes */

static int get_local_leader(void);

static int get_edge_weight(int robot1, int robot2);

static void get_bom_velocity(int bom_id, int* turn_speed, int* speed);

void lemmings_init() {

  // usb_puts("lemmings_init\n");

  //  colonet_init();

  //run_around_init(); // prepare for moving

  // set to defaults

  cur_state = FOLLOW;

  local_leader = -1;

  global_leader = -1;

  local_id = wl_get_xbee_id();

  follow_multi = FOLLOW_MULTI_DEFAULT;

}

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

void lemmings_FSM(void) {

  int leader_bom = 0, turn_speed = 0, speed = 0;

  static int leader_check_count = 0;

  if (wl_token_get_num_robots() <= 1) {

    /* if there is only one robot in the token ring, there is nothing to

       follow, so don't do anything */

    motors_off();

    return;

  }

  //usb_puts("state: ");

  //usb_puti(cur_state);

  //usb_puts("\n");

  // only check for new global leader once every 5 iterations

  leader_check_count++;

  if (leader_check_count % 5 == 0) {

    wl_token_iterator_begin();

    // find robot with max id and assign as leader

    int max = -1;

    while (wl_token_iterator_has_next()) {

      int n = wl_token_iterator_next();

      if (n > max) {

        max = n;

      }

/*       usb_puti(n); */

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

    }

/*     usb_puti(local_id); */

/*     usb_puts(" is the local robot\n"); */

    if (max == local_id) {

      // become the global leader

      orb_set_color(BLUE);

      cur_state = LEAD;

      local_leader = -1;

//      usb_puts(" becoming leader \n");

    } else {

      // become a follower

      orb_set_color(RED);

      cur_state = FOLLOW;

//      usb_puts(" becoming follower\n");

    }

    global_leader = max; // set the global leader

  }

  switch (cur_state) {

  case LEAD:

    // do lead action - move straight ahead

    orb_set_color(BLUE);

    move(SPEED, 0);

    break;

  case FOLLOW:

    orb_set_color(RED);

    // Make sure we can see our local leader

    leader_bom = (local_leader>=0)?wl_token_get_sensor_reading(local_id, local_leader):-1;

    if (leader_bom < 0) {

      //usb_puts("no connection to leader\n");

      // Can't see our old leader -- get new bot to follow.

      local_leader = get_local_leader();

      if (local_leader == -1) {

        // nothing to follow, so become a local_leader and start leading

        //usb_puts("can't see anyone - becoming a leader\n");

        cur_state = LEAD;

        local_leader = -1;

        orb_set_color(BLUE);

        move(SPEED, 0);

        return;

      } else {

        //usb_puts("new leader is");

        //usb_puti(local_leader);

        //usb_puts("\n");

        // get data in order to follow leader

        leader_bom = wl_token_get_sensor_reading(local_id, local_leader);

        get_bom_velocity(leader_bom, &turn_speed, &speed);

      }

    } else {

      // Leader is still in sight, so follow it

      get_bom_velocity(leader_bom, &turn_speed, &speed);

    }

  //usb_puts("following robot# ");

  //usb_puti(local_leader);

  //usb_puts(" bom of leader: ");

  //usb_puti(leader_bom);

  //usb_puts("\n");

    move(speed, turn_speed); // move in direction of leader

    break;

  }

}

// translate bom reading into direction and speed

static void get_bom_velocity(int bom_id, int* turn_speed, int* speed) {

  int error = 0;

  if (bom_id < 0 || bom_id > 15) {

    //   usb_puts("ERROR - invalid bom_id\n");

    *turn_speed = 0;

    *speed = 0;

    return;

  }

  // set so right is negative, left is positive

  if (bom_id <= 12) {

    error = bom_id - 4;

  } else {

    error = bom_id - 20;

  }

  // set turn speed and main speed

  // if error is small, turn slowly and travel at normal speed

  // if error is large, turn faster and travel at reduced speed

  switch(error)

  {

  case 0:

    *turn_speed = 0;

    *speed = SPEED;

    return;

  case 1:

  case -1:

    *turn_speed = error * 5;

    *speed = SPEED;

    return;

  case 2:

  case -2:

    *turn_speed = error * 8;

    *speed = SPEED;

    return;

  case 3:

  case -3:

    *turn_speed = error * 12;

    *speed = SPEED;

    return;

  case 4:

  case -4:

    *turn_speed = error * 16;

    *speed = SPEED - SPEED * error * error / 36;

    return;

  }

  // if not in a case

  *turn_speed = error * 20;

  *speed = SPEED - SPEED * error * error / 36;

}

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

// CURRENTLY JUST LOOKS FOR CLOSEST ROBOT TO FOLLOW

static int get_local_leader(void) {

  int nodeB;

  int cur_weight;

  int best_weight = 1000; // set to infinity initially

  int best_node = -1;

  wl_token_iterator_begin();

  // iterator through robots in token ring

  while (wl_token_iterator_has_next()) {

    nodeB = wl_token_iterator_next();

    if (nodeB == local_id) {

      continue; // can't follow self

    }

    // get an edge weight based on the robot position

    cur_weight = get_edge_weight(local_id, nodeB);

    if (nodeB == global_leader && cur_weight != -1) {

      // always follow the global leader if you have the option to

      best_node = nodeB;

      break;

    }

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

      // this is new best node, so save values.

      best_weight = cur_weight;

      best_node = nodeB;

    }

  }

  return best_node;

}

// get edge weight of sensor matrix

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

static int get_edge_weight(int robot1, int robot2) {

  int bom = wl_token_get_sensor_reading(robot1,robot2);

  // Robots closer to directly in front of us have lower weight.

  switch(bom) {

  case 12:

    return 10;

  case 13:

  case 11:

    return 9;

  case 14:

  case 10:

    return 8;

  case 15:

  case 9:

    return 7;

  case 0:

  case 8:

    return 6;

  case 1:

  case 7:

    return 5;

  case 2:

  case 6:

    return 4;

  case 3:

  case 5:

    return 3;

  case 4:

    return 2;

  }

  return -1;

}