Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include <wl_basic.h>

#include "smart_run_around_fsm.h"

#define CHAN 0xF

#define TAG_TIME 5

#define TAG_RANGE 150

#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;

void test_bom() {

  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;

  uint8_t i;

  int onTarget = 0;

  char packet[2];

  uint8_t bom = 0;

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

  color = 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");

      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(','); */

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

      if(angle < 7 && angle > 1 && 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;

              bom_on();

            }

            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;

      }

    }

    wl_do();

  }

  return 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;

        bom_off();

        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');

  }

}