Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#define CHAN 0xF

#define TAG_TIME 3000

#define TAG_RANGE 150

int main(void) {

  int frontRange;

  uint8_t angle;

  uint8_t i;

  uint8_t isPrey = 0;

  int onTarget = 0;

  uint8_t color;

  //intialize the robot and wireless

  dragonfly_init(ALL_ON);

  wl_init();

  bom_init(BOM15);

  //use a specific channel to avoid interfering with other tasks

  wl_set_channel(CHAN);

  orb_set_color(BLUE);

  color = BLUE;

  while(1) {

    bom_refresh(BOM_ALL);

    frontRange = range_read_distance (IR2);

    usb_puti(frontRange);

    usb_putc('\n');

    //BUG: when get_max_bom is called, the rangefinder seems to stop

    //working

    angle = get_max_bom();

    angle = 4;

    if(angle == 4 && frontRange > 0 && frontRange < TAG_RANGE) {

      if(onTarget == 0) {

        onTarget = TAG_TIME;

        if(color != GREEN) {

          color = GREEN;

          orb_set_color(GREEN);

        }

        usb_puts("On target!\n");

      }

      else {

        if(--onTarget <= 0) {

          orb_set_color(RED);

          color = RED;

          usb_puts("TAG!\n");

          while(1);

        }

      }

    }

    else{

      if(color != BLUE) {

        color = BLUE;

        orb_set_color(BLUE);

      }

      //don't reset onTarget because the robot got too close

      if(frontRange > 0)

        onTarget = 0;

    }

    wl_do();

  }

  return 0;

}