Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include <wl_basic.h>

#include "smart_run_around_fsm.h"

#include "hunter_prey.h"

#define CHAN 0xC

#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 color1, color2;

volatile uint8_t tagAck;

volatile uint8_t tagger;

uint8_t 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 > 16)

    return;

  if(omega >  8)

    omega = omega - 16;

  omega *= 255/8;

/*   usb_puti(omega); */

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

  move(140, omega);

}

int main(void) {

  int frontRange;

  uint8_t angle;

  unsigned char packet[2];

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

  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_puts("orb set\n");

  orb_set_color(BLUE);

  color1 = BLUE;

  color2 = BLUE;

  //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) {

      run_around_FSM();

    }

    else { //ROBOT_HUNTER

      bom_refresh(BOM_ALL);

      delay_ms(100);

      frontRange = range_read_distance (IR2);

/*       usb_puti(frontRange); */

/*       usb_putc(','); */

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

      //working

      angle = bom_get_max();

/*       usb_puti(angle); */

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

      chase(angle);

      //debugging to determine tagging conditions

      if(angle < 7 && angle > 1) {

        if(color1 != RED) {

          color1 = RED;

          orb1_set_color(RED);

        }

      }

      else {

        if(color1 != BLUE) {

          color1 = BLUE;

          orb1_set_color(BLUE);

        }

      }

      usb_puti(frontRange);

      usb_putc('\n');

/*       if(frontRange > 0 && frontRange < TAG_RANGE) { */

/*         if(color2 != RED) { */

/*           color2 = RED; */

/*           orb2_set_color(RED); */

/*         } */

/*       } */

/*       else { */

/*         if(color2 != BLUE) { */

/*           color2 = BLUE; */

/*           orb2_set_color(BLUE); */

/*         } */

/*       } */

      if(hunter_prey_tagged(angle, frontRange) || button1_read()) {

        orb_set_color(YELLOW);

        color1 = YELLOW;

        color2 = YELLOW;

        usb_puts("TAG!\n");

        tagAck = 0;

        packet[0] = HUNTER_PREY_ACTION_TAG;

        packet[1] = id;

        wl_basic_send_global_packet (42, &packet, 2);

        move(0,0);

        while(!tagAck)

          wl_do();

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

          usb_puts("ACK!\n");

          move(-230,0); //back up to give the new prey some room

          delay_ms(TAG_PAUSE/2);

          move(0,0);

          robotState = ROBOT_PREY;

          bom_on();

        }

        else {

          usb_puts("ACK failed!\n");

        }

      }

    }

    wl_do();

  }

  return 0;

}

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

{

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

    if(packet[0] == HUNTER_PREY_ACTION_TAG) {

      if(robotState == ROBOT_PREY) {

        robotState = ROBOT_TAGGED;

        bom_off();

        color1 = BLUE;

        color2 = BLUE;

        orb_set_color(BLUE);

        tagger = packet[1];

      }

      else {

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

      }

    }

    else if(packet[0] == HUNTER_PREY_ACTION_ACK && 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');

  }

}

#define HP_BUF_IDX_ACTION 0