Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include <wl_basic.h>

#include "smart_run_around_fsm.h"

#include "hunter_prey.h"

#define CHAN 0xF

#define ACK_PAUSE 5000

#define ROBOT_HUNTER 0

#define ROBOT_PREY 1

#define ROBOT_TAGGED 2

#define ROBOT_IDLE 3

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

volatile uint8_t robotState; // current state

volatile uint8_t tagger;    // id of tagging robot

uint8_t id; // my robot's id

void test_bom(void) {

  int i;

  int val;

  while(1) {

    bom_refresh(BOM_ALL);

    for(i=0; i<16; i++) {

      val = bom_get(i);

      usb_puti(val);

      usb_putc(' ');

    }

    usb_puts("| ");

    usb_puti(range_read_distance (IR1));

    usb_putc(' ');

    usb_puti(range_read_distance (IR2));

    usb_putc(' ');

    usb_puti(range_read_distance (IR3));

    usb_putc(' ');

    usb_puti(range_read_distance (IR4));

    usb_putc(' ');

    usb_puti(range_read_distance (IR5));

    usb_putc('\n');

    delay_ms(500);

  }

}

void chase(uint8_t angle) {

  int omega = angle - 4;

  if(angle > 15)

    return;

  if(omega >  8)

    omega = omega - 16;

  omega *= 255/8;

/*   usb_puti(omega); */

/*   usb_putc('\n'); */

  move(140, omega);

}

int main(void) {

  int frontRange;   // reading of front rangefinder

  uint8_t angle;    // result of bom_get_max()

  unsigned char *packet;  // packet to send

  int length;        // length of received packet

  //intialize the robot and wireless

  dragonfly_init(ALL_ON);

  //  test_bom();

  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_default();

  usb_puts("run around.\n");

  run_around_init();

  usb_puts("channel\n");

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

  wl_set_channel(CHAN);

  usb_puti(CHAN);

  usb_puts("orb set\n");

  // initially in hunter state

  robotState = ROBOT_HUNTER;

  //if button 2 is held, you are prey

  if(button2_read()) {

    robotState = ROBOT_PREY;

    bom_on();

  }

  while(1) {

    if(robotState==ROBOT_TAGGED) {

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

      //delay_ms(TAG_PAUSE);

      move(0,0);

      usb_puts("sending ACK to ");

      usb_puti(tagger);

      usb_putc('\n');

      packet[0] = HUNTER_PREY_ACTION_ACK;

      packet[1] = tagger;

      wl_basic_send_global_packet (42, &packet, 2);

      usb_puts("sent\n");

      robotState = ROBOT_HUNTER; //no longer prey

    }

    else if(robotState == ROBOT_PREY) {

        orb_set_color(GREEN);

      run_around_FSM();

    }

    else if (robotState == ROBOT_IDLE) {

        orb_set_color(BLUE);

        move(0,0);

        delay_ms(ACK_PAUSE);

        robotState = ROBOT_HUNTER;

    }

    else { //ROBOT_HUNTER

        orb_set_color(RED);

      bom_refresh(BOM_ALL);

      delay_ms(100);

      frontRange = range_read_distance (IR2);

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

      //working

      angle = bom_get_max();

      chase(angle);

      usb_puti(frontRange);

      usb_putc('\n');

      if ((hunter_prey_tagged(angle, frontRange) && rtc_get() >= 1) || button1_read()) {

            usb_puts("TAG!\n");

            packet[0] = HUNTER_PREY_ACTION_TAG;

            packet[1] = id;

            wl_basic_send_global_packet (42, &packet, 2);

            rtc_reset();

        }

    }

    packet = wl_basic_do_default(&length);

    if (packet)

        packet_receive(packet, length);

  }

  return 0;

}

void packet_receive(unsigned char* packet, int length)

{

  if(length>=2){

    if(packet[0] == HUNTER_PREY_ACTION_TAG) {

      if(robotState == ROBOT_PREY) {

        robotState = ROBOT_TAGGED;

        bom_off();

        tagger = packet[1];

      }

      else {

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

      }

    }

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

      robotState = ROBOT_IDLE;

    }

    usb_puts("got '");

    usb_putc(packet[0]);

    usb_puti(packet[1]);

    usb_puts("'\n");

  }

  else {

    usb_puts(" length=");

    usb_puti(length);

    usb_putc('\n');

  }

}

#define HP_BUF_IDX_ACTION 0