Colony

#include <wl_basic.h>

#include "smart_run_around_fsm.h"

#define TAG_TIME 10

#define TAG_PAUSE 2000

#define ROBOT_HUNTER 0

#define ROBOT_PREY 1

#define ROBOT_TAGGED 2

void packet_receive(char type, int source, unsigned char* packet, int length);

volatile uint8_t robotState = ROBOT_HUNTER;

volatile uint8_t color;

volatile uint8_t tagAck;

volatile uint8_t tagger;

uint8_t id;

  char packet[2];

  id = get_robotid();

  packet[1] = id;

  usb_puts("hello, I am robot \n");

  usb_puti(id);

  usb_putc('\n');

  usb_puts("wireless.\n");

  wl_basic_init(&packet_receive);

  usb_puts("run around.\n");

  run_around_init();

  //  bom_init(BOM15);

  usb_puts("channel\n");

  usb_puts("orb set\n");

  //if button 2 is held, you are prey

  if(button2_read()) {

    robotState = ROBOT_PREY;

  }

    if(robotState==ROBOT_TAGGED) {

      usb_puts("tagged, waiting to send ACK\n");

      move(0,0);

      delay_ms(TAG_PAUSE);

      usb_puts("sending ACK to ");

      usb_puti(tagger);

      usb_putc('\n');

      packet[0] = 'A';

      packet[1] = tagger;

      wl_basic_send_global_packet (0, packet, 2);

      usb_puts("sent\n");

      move(0,0);

      robotState = ROBOT_HUNTER; //no longer prey

    }

    else if(robotState == ROBOT_PREY) {

      run_around_FSM();

    }

    else { //ROBOT_HUNTER

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

	    color = YELLOW;

	    usb_puts("TAG!\n");

	    tagAck = 0;

	    packet[0] = 'T';

	    packet[1] = id;

	    wl_basic_send_global_packet (0, packet, 2);

	    move(0,0);

	    while(!tagAck)

	      wl_do();

	    if(tagAck == id) { //if the ack was for us

	      usb_puts("ACK!\n");

	      //	      delay_ms(TAG_PAUSE);

	      robotState = ROBOT_PREY;

	    }

	    else {

	      usb_puts("ACK failed!\n");

	    }

	  }

	}

      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;

void packet_receive(char type, int source, unsigned char* packet, int length)

{

  if(type==0 && length>=2){

    if(packet[0] == 'T') {

      if(robotState == ROBOT_PREY) {

	robotState = ROBOT_TAGGED;

	color = BLUE;

	orb_set_color(BLUE);

	tagger = packet[1];

      }

      else {

	usb_puts("tagged, but I'm not it!\n");

      }

    }

    else if(packet[0] == 'A' && robotState == ROBOT_HUNTER) {

      tagAck = packet[1];

    }

    else {

      usb_puts("got '");

      usb_putc(packet[0]);

      usb_puti(packet[1]);

      usb_puts("'\n");

    }

  }

  else {

    usb_puts("got unkown packet! type=");

    usb_puti(type);

    usb_puts(" length=");

    usb_puti(length);

    usb_putc('\n');

  }

}