Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include <wl_token_ring.h>

#include <queue.h>

#include <string.h>

#include "bfs_fsm.h"

#include "smart_run_around_fsm.h"

//#include "orbit_fsm.h"

/* Used to find a robot (or other object)

  Uses bom and token ring

  Assumes:

    robots are already in a token ring

*/

/* private function prototypes */

void evaluate_state(void);

int bfs_follow(void);

/* init */

void bfs_init(int robot) {

  range_init();

  analog_init(1);

  motors_init();

  orb_init();

  orb_enable();

  //usb_init();

  run_around_init();

  /*Start in the start state, BFS_SEEK */ 

  bfs_state = BFS_SEEK;

  bfs_otherRobot = robot; // set which robot to seek

  bfs_my_id = wl_get_xbee_id();

  bfs_follow_id = -1;

  bfs_pControl=0;

  bfs_bom = 0;

  /*Initialize distances to zero.*/ 

  bfs_d1=1000; bfs_d2=1000; bfs_d3=1000; bfs_d4=1000; bfs_d5=1000;

}

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

void bfs_fsm(void) {

  /*The following lines ensure that undefined (-1) values

  will not update the distances.*/ 

  int temp;  

  wl_do(); // update wireless

  if (bfs_state == BFS_SEEK || bfs_state == BFS_FOLLOW) {

    bfs_follow_id = bfs_follow();

    if (bfs_follow_id == BFS_NO_VAL)

      bfs_state = BFS_SEEK; // move to seek state

    else {

      // found robot to follow to goal

      bfs_state = BFS_FOLLOW;

      // get bom reading

      temp = wl_token_get_my_sensor_reading(bfs_follow_id);

      bfs_bom = (temp == -1) ? bfs_bom : temp;

      // modify bom reading so right is negative, left is positive

      if (bfs_bom <= 12)

        bfs_bom -= 4;

      else

        bfs_bom -= 20;

      bfs_pControl = bfs_bom*4;

      // get range reading for front

      temp=range_read_distance(IR2);

      bfs_d2=(temp == -1) ? bfs_d2 : temp;

      if (bfs_d2 < BFS_ORBIT_DISTANCE) {

        bfs_state = BFS_ORBIT; // move to orbit state

        //orbit_init(bfs_follow_id);

      }

    }  

  }

  else if (bfs_state == BFS_ORBIT) {

      // get range reading for front

      temp=range_read_distance(IR2);

      bfs_d2=(temp == -1) ? bfs_d2 : temp;

      if (bfs_d2 < BFS_ORBIT_DISTANCE)

        bfs_state = BFS_STOP; // move to stop state  

  }

  // evaluate state

  evaluate_state();

}

//Acts on state change.

void evaluate_state(){

    switch(bfs_state){

    case(BFS_SEEK): orb_set_color(RED);

      // move around

      run_around_FSM(); // note: better to just incorporate into this file later one

      break;

    case(BFS_FOLLOW): orb_set_color(GREEN);

      // follow another robot

      move(BFS_STRAIGHT_SPEED,bfs_pControl);

      break;

    case(BFS_ORBIT): orb_set_color(BLUE);

      // orbit a robot

      //orbit_fsm();

      move(0,0);

      break;

    case(BFS_STOP): orb_set_color(YELLOW);

      // stop

      move(0,0);

      break;

    default: orb_set_color(YELLOW);

      /*Should never get here, so stop.*/

      move(0,0);

      break;

  }

}

/* find a robot to follow using BFS 

  ported from colonybfs by Felix

*/

int bfs_follow()

{

/* pseudocode for BFS

procedure bfs(v)

    q := make_queue()

    enqueue(q,v)

    mark v as visited

    while q is not empty

        v = dequeue(q)

        process v

        for all unvisited vertices v' adjacent to v

            mark v' as visited

            enqueue(q,v')

*/

  Queue* q = queue_create();

  //int num_current_robots = wl_token_get_robots_in_ring();

  // keep track of which nodes you have visited.  Indexed by robot #

  // 1 if visited, 0 if not

  unsigned char visited_nodes[BFS_MAX_ROBOTS];

  // keep track of the distance from the start robot to other robots

  // also indexed by robot#

  unsigned char node_distances[BFS_MAX_ROBOTS];

  // this is the path you take

  unsigned char path[BFS_MAX_ROBOTS];

  //variables you will need

  unsigned char current_node;                 //current node

  unsigned char current_neighbour_node;       //neighbor to the current node

  unsigned char current_neighbour_val;        //value of that neighbour's sensors

  unsigned char current_distance;              //keep track of current distance to the start

  unsigned char large_number = 255;

  unsigned char* add_value;

  //set visited nodes to all 0

  memset(visited_nodes, 0, BFS_MAX_ROBOTS);

  //set all the distances to a very large number

  //this isn't technically necessary, but it's probably a good thing -- just in case

  memset(node_distances, large_number, BFS_MAX_ROBOTS);

  //set the path to all LARGE_NUMBER as well

  memset(path, large_number, BFS_MAX_ROBOTS);

  //queue_remove_all(q);

  //add the start node

  add_value = (unsigned char*)malloc(sizeof(char));

  (*add_value) = bfs_my_id;

  queue_add(q, add_value);

  visited_nodes[bfs_my_id] = 1;

  node_distances[bfs_my_id] = 0;

  while(!queue_is_empty(q)){

    add_value = queue_remove(q);

    current_node = (*add_value);

    //get the current distance from you to the start

    current_distance = node_distances[current_node];

    //this node is on your 'path'

    path[current_distance] = current_node;

    //note: it's OK if this path leads nowhere -- the path will be

    //overwritten by a node that does lead to the goal

    //(if there is no path, we set path to null anyways)

    //from now on, all further nodes will be one further away -- increment

    current_distance++;

    //process node -- basically check if it's the goal

    if(current_node == bfs_otherRobot) {

      //you reach the goal

      //return the first robot in the path, which is the one

      //to follow

      /*

      lcd_putchar('.');

      lcd_putchar('.');

      for(i = 0; i < MAX_ROBOTS; i++)

          lcd_putchar(path[i] +48);           //print out the path for fun -- remove this later

      lcd_putchar('.');

      lcd_putchar('.');

      */

      return path[1];         //path[0] is you.  path[1] is who you want to follow

    }

    // go through all nodes adjacent to current

    // in effect, this means going through the current node's row in the matrix

    wl_token_iterator_begin();

    while(wl_token_iterator_has_next()) {

      //the robot # is actually just the index

      current_neighbour_node = wl_token_iterator_next();

      //the value is what is stored in the matrix (this is important)

      current_neighbour_val = wl_token_get_sensor_reading(current_node,current_neighbour_node);

      //check for connected-ness and that it was not visited

      //            unconnected                           visited

      if( (current_neighbour_val == BFS_NO_VAL) || (visited_nodes[current_neighbour_node]) ) {

        //if it is either unconnected or previously visited, exit

        continue;   //go to the next node

      }

      //update the distance from the start node to this node

      node_distances[current_neighbour_node] = current_distance;

      //also mark it as visited

      visited_nodes[current_neighbour_node] = 1;

      //also enqueue each neighbour

      add_value = (unsigned char*)malloc(sizeof(char));

      (*add_value) = current_neighbour_node;

      queue_add(q, add_value);

    }

  }

  //if we get to here, there is no path

  memset(path, 0, BFS_MAX_ROBOTS);

  return BFS_NO_VAL;

}