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

  orbit_theta_stop requires encoders, and is incomplete

  Assumes:

    both robots are already in a token ring

*/

/* private function prototype */

void evaluate_state(void);

/* primary init */

void orbit_init(int robot) {

  range_init();

  analog_init(1);

  motors_init();

  orb_init();

  orb_enable();

  //usb_init();

  /*Start in the start state, ORBIT_SEEK */

  orbit_state = ORBIT_SEEK;

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

  orbit_pControl=0;

  orbit_bom = 0;

  orbit_theta = 0;

  orbit_theta_stop = 1000;

  /*Initialize distances to zero.*/

  orbit_d1=1000; orbit_d2=1000; orbit_d3=1000; orbit_d4=1000; orbit_d5=1000;

}

/* secondary init */

void orbit_init_theta(int robot,int theta) {

  range_init();

  analog_init(1);

  motors_init();

  orb_init();

  orb_enable();

  //usb_init();

  /*Start in the start state, ORBIT_SEEK */

  orbit_state = ORBIT_SEEK;

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

  orbit_pControl=0;

  orbit_bom = 0;

  orbit_theta = 0;

  orbit_theta_stop = (theta>0)?theta:-1; // check theta_stop

  /*Initialize distances to zero.*/

  orbit_d1=1000; orbit_d2=1000; orbit_d3=1000; orbit_d4=1000; orbit_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;

  temp=range_read_distance(IR1);

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

  temp=range_read_distance(IR2);

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

  temp=range_read_distance(IR3);

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

  temp=range_read_distance(IR4);

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

  temp=range_read_distance(IR5);

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

  wl_do(); // update wireless

  // get bom reading

  temp = wl_token_get_my_sensor_reading(orbit_otherRobot);

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

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

  if (orbit_bom <= 12)

    orbit_bom -= 4;

  else

    orbit_bom -= 20;

  if (orbit_state == ORBIT_SEEK && orbit_d2 < ORBIT_DISTANCE)

    orbit_state = ORBIT_INSERTION; // begin to orbit

  if (orbit_state == ORBIT_INSERTION && orbit_bom == ORBIT_DIRECTION)

    orbit_state = ORBIT_ORBITING; // orbit achieved

  if (orbit_state == ORBIT_ORBITING && ((orbit_d2 < ORBIT_DISTANCE)

    || (orbit_theta >= orbit_theta_stop)))

    orbit_state = ORBIT_STOP; // orbit obstructed

  // evaluate state

  evaluate_state();

}

//Acts on state change.

void evaluate_state(){

    switch(orbit_state){

    case(ORBIT_SEEK): orb_set_color(RED);

      // move towards robot

      orbit_pControl = orbit_bom*10;

      move(ORBIT_STRAIGHT_SPEED,orbit_pControl);

      break;

    case(ORBIT_INSERTION): orb_set_color(GREEN);

      // rotate into orbit, perpendicular to other robot

      orbit_pControl = -ORBIT_DIRECTION*10;

      move(ORBIT_STRAIGHT_SPEED/3,orbit_pControl);

      break;

    case(ORBIT_ORBITING): orb_set_color(BLUE);

      // go straight with slight rotation

      if (orbit_bom == ORBIT_DIRECTION)

        orbit_pControl = ORBIT_DIRECTION;

      else if (orbit_bom < ORBIT_DIRECTION)

        orbit_pControl = ORBIT_DIRECTION-ORBIT_CORRECTION;

      else

        orbit_pControl = ORBIT_DIRECTION+ORBIT_CORRECTION;

      move(ORBIT_STRAIGHT_SPEED,orbit_pControl);

      break;

    case(ORBIT_STOP): orb_set_color(YELLOW);

      move(0,0);

      break;

    default: orb_set_color(YELLOW);

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

      move(0,0);

      break;

  }

}