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

static int speed;         // robot's forward speed

static int turn_speed;    // robot's turning speed

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

void lemmings_init() {

  // usb_puts("lemmings_init\n");

  //  colonet_init();

  //run_around_init(); // prepare for moving

  //range_init();

  //analog_init();

  /* range finder code interferes with token ring */

  orb_init();

  orb_enable();

  // set to defaults

  cur_state = FOLLOW;

  local_leader = -1;

  global_leader = -1;

  local_id = wl_get_xbee_id();

  speed = SPEED;

  turn_speed = 0;

  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;

  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();

    speed = 0;

    orb_set_color(GREEN);

    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 % 3 == 0) {

    if (cur_state == LEAD && leader_check_count % 12 == 0) {

      // find robot with max id and assign as leader

      int max = -1;

      wl_token_iterator_begin();

      while (wl_token_iterator_has_next()) {

        int n = wl_token_iterator_next();

        if (n > max) {

          max = n;

        }

  /*       usb_puti(n); */

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

      }

      if (max == local_id) {

        // become the global leader

        orb_set_color(BLUE);

        cur_state = LEAD;

        local_leader = -1;

        speed = SPEED;

        turn_speed = 0;

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

    }

    if (cur_state == FOLLOW) {

      // 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) {

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

            follow, so don't do anything */

          /*motors_off();

          speed = 0;

          return;*/

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

          speed = SPEED;

          turn_speed = 0;

        } 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);

        }

      } else {

        // Leader is still in sight, so follow it

        get_bom_velocity(leader_bom);

      }

    }

  }

  else {

    // prevent spinning in circles

    turn_speed /= 5;

  }

  // check if running into something

  /*int range = range_read_distance(IR2);

  if (range > 0 && range < 150) {

    if (cur_state == LEAD) {

      turn_speed = 100; // turn to avoid

    }

    else {

      speed = 0; // stop to avoid

    }

  }*/

  // do movement

  move(speed, turn_speed);

}

// translate bom reading into direction and speed

static void get_bom_velocity(int bom_id) {

  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;

}