Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include <wl_token_ring.h>

#include "orbit_fsm.h"

/* Used to orbit a robot

  Must be within BOM range of robot before activating

  Assumes:

    both robots are already in a token ring

*/

void orbit_init(int robot) {

  range_init();

  analog_init();

  motors_init();

  orb_init();

  orb_enable();

  //usb_init();

  /*Start in the start state, SEEK */ 

  state = SEEK;

  otherRobot = robot; // set which robot to seek and orbit

  pControl=0;

  bom = 0;

  /*Initialize distances to zero.*/ 

  d1=1000; d2=1000; d3=1000; d4=1000; d5=1000;

}

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

void orbit_fsm(void) {

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

  will not update the distances.*/ 

  int temp;  

  wl_do(); // update wireless

  temp=range_read_distance(IR1);

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

  temp=range_read_distance(IR2);

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

  temp=range_read_distance(IR3);

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

  temp=range_read_distance(IR4);

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

  temp=range_read_distance(IR5);

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

  // get bom reading

  temp = wl_token_get_my_sensor_reading(otherRobot);

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

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

  if (bom <= 12)

    bom -= 4;

  else

    bom -= 20;

  if (state == SEEK && d2 < ORBIT_DISTANCE)

    state = TO_ORBIT; // begin to orbit

  if (state == TO_ORBIT && bom == ORBIT_DIRECTION)

    state = ORBIT; // orbit achieved

  if (state == ORBIT && d2 < ORBIT_DISTANCE)

    state = STOP; // orbit obstructed

  // evaluate state

  evaluate_state();

}

//Acts on state change.

void evaluate_state(){

    switch(state){

    case(SEEK): orb_set_color(RED);

      // move towards robot

      pControl = bom*10;      

      move(STRAIGHT_SPEED,pControl);

      break;

    case(TO_ORBIT): orb_set_color(GREEN);

      // rotate into orbit, perpendicular to other robot

      pControl = -ORBIT_DIRECTION*10;

      move(STRAIGHT_SPEED/3,pControl);

      break;

    case(ORBIT): orb_set_color(BLUE);

      // go straight with slight rotation

      if (bom == ORBIT_DIRECTION)

        pControl = ORBIT_DIRECTION*2;

      else if (bom < ORBIT_DIRECTION)

        pControl = ORBIT_DIRECTION-10;

      else

        pControl = ORBIT_DIRECTION+10;

      move(STRAIGHT_SPEED,pControl);

      break;

    case(STOP): orb_set_color(YELLOW);

      move(0,0);

      break;

    default: orb_set_color(YELLOW);

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

      move(0,0);

      break;

  }

}