Colony

/**

   \file wl_adhoc.c

   Wireless ad hoc networking.

   NOW WITH SENSORS

   Robots can come in and out of the ring.

*/

#include <dragonfly_lib.h>

#include "wl_adhoc.h"

#include <avr/interrupt.h>

#include "ring_buffer.h"

#include "bom.h"

#include <colonet_dragonfly.h>

#include <colonet_defs.h>

#define MAX(a, b) ((a) > (b) ? (a) : (b))

#define MIN(a, b)  ((a) < (b) ? (a) : (b))

//#define USB_DEBUG

#ifdef USB_DEBUG

#define USB_PUTC( c ) usb_putc( c )

#define USB_PUTS( s ) usb_puts( s )

#define USB_PUTI( i ) usb_puti( i )

#else

#define USB_PUTC( c )

#define USB_PUTS( s )

#define USB_PUTI( i )

#endif

RING_BUFFER_NEW(ring_buffer, 256, char, buffer);

volatile int buffer_size = 0;

void led_user(int x) {}

void usb_putstr(char *s)

{

  char *t = s;

  while (*t != 0)

  {

    usb_putc(*t);

    t++;

  }

}

unsigned char set_orb_wireless = 1;

unsigned char crazy_debug = 0;

int max_bom = -1;

/*

  \fn wl_timeout_handler

  Handles the timeout case.

  A time out occurs about every 1/4 second.  This function is called each time 

  one occurs.  In the case where a robot first turns on, it will wait for a 

  long time before declaring itself the leader.  Otherwise, if enough 

  time-outs occur in a row, wl_to_flag will be set to 1.  This will cause a 

  timeout packet to be sent next time around

*/

void wl_timeout_handler( void ) {

  if(crazy_debug) {

    return;

  }

  //increment the total number of wireless time-outs

  wl_to_count++;

  //lcd_putchar('w');

  USB_PUTC('w');

  static int wl_first_to_count =  0;

  //first time time out routine - listen for a long time

  if(first_time) {

    if(wl_to_count > WL_TO_INITIAL){

      //increment the counter for first timeouts

      wl_first_to_count++;

      //lcd_putchar('Z');

      //you don't actually want to send out a packet -- you would confuse 

      //other robots

      wl_to_flag = 0;

      wl_to_count = 0;

    }

  }else{

    //not the first time - regular time out routine

    //

    if ((wl_sent_cs || wl_sent_depart || wl_sent_dock) && wl_to_count > 3)

    {

      usb_puts("Charging station timed out. \n\r");

      wl_sent_cs = 0;

      wl_sent_depart = 0;

      wl_sent_dock = 0;

      wl_create_packet();

      wl_send();

    }

    //if you have enough timeouts

    if(wl_to_count > WL_TIMEOUT){

      //lcd_putchar('z');

      USB_PUTS("|time.out|");

      //change the time-out flag to 1

      wl_to_flag = 1;

      wl_to_count = 0;

      //increment the count for robot death

      if(NUM_BOTS > 1) {

        //only increase death count if there are robots that can die

        //if numbots = 1, you are the only robot (no suicidal robots... yet)

        wl_to_death_count++;

      }

      //check for a robot death

      if(wl_to_death_count > WL_TO_DEATH) {

        //a robot has died :(

        wl_to_death_flag = 1;

        wl_death_handler();

      }

    }

  }

  //take care of the case where you time out so much that you become the leader

  if(wl_first_to_count > WL_TO_LEADER) {

    //you are now the leader

    //lcd_putchar('L');

    first_time = 0;

    NUM_BOTS = 1;     //there are (1) robot in the ring

    SRC = 0;        //you are robot # 1

    DEST = 1;       //you are 'speaking' to (nonexistent) robot #1

    /*

      lcd_putchar('[');

      lcd_putchar(SRC+48);

      lcd_putchar('|');

      lcd_putchar(DEST+48);

      lcd_putchar('|');

      lcd_putchar(NUM_BOTS+48); 

      lcd_putchar('|');

      lcd_putint(wl_to_death_count);

      lcd_putchar(']');

    */

    usb_putc('t');

    //create a a packet and send it

    wl_create_packet();

    wl_send();

    //change the timeout count to 0

    wl_to_count = 0;

    wl_first_to_count = 0;

  }

}

/**

   \fn wl_error_handler(int errcode)

   Handles error codes.

   \param errcode The error code.

   At the time, the only real error that happens is robot death.

   Robot death requires that wl_buf have been set (either by receiving a

   error packet or by calling wl_death_handler.

   Robot death:

   Number of robots will decrement.

   Robots above the dead robot will decrement their ID and their source.

   The loop remains closed.

*/

void wl_error_handler(unsigned char errcode)

{

  USB_PUTS("Error Code: ");

  USB_PUTI(errcode);

  //which robot is dead

  int dead_robot;

  //print debug information

  /*

    lcd_clear_screen();

    lcd_gotoxy(0,0);

    lcd_putstr("error handler    ");

    lcd_putint(errcode);

    lcd_gotoxy(0,1);

  */

  USB_PUTS("|error handler|");

  switch(errcode) {

  case WL_ERRCODE_ROBOTDEATH:

    orb_set_color(RED);

    buzzer_chirp(1000, C5);

    //lcd_putstr("  dead_robot: ");

    //lcd_putint(wl_buf[WL_ERR_DEAD_LOC]);

    break;

  case WL_ERRCODE_NEEDTOCHARGE:

    //lcd_putstr("   charge_robot: ");

    //lcd_putint(wl_buf[WL_SRC_LOC]);

    break;

  default:

    break;

  }

  //lcd_gotoxy(0,2);

  //set all the time-out counts and flags to 0

  wl_to_count = 0;

  wl_to_flag = 0;

  wl_to_death_flag = 0;

  wl_to_death_count = 0;

  //acknowledge receipt of the error packet by setting the errorcode in the 

  //packet to 0

  wl_buf[WL_ERROR_LOC] = 0;

  //also set checksum to !0

  wl_chksum = 1;

  //switch the error code

  switch(errcode) {

    //ROBOT DEATH

  case WL_ERRCODE_ROBOTDEATH:

    //get which robot has died

    dead_robot = wl_buf[WL_ERR_DEAD_LOC];

    //decrement the total number of robots

    NUM_BOTS--;

    //see if you are above the dead robot

    if( SRC > dead_robot ) {

      //if you are the last robot (since NUM_ROBOTS was decremented, 

      //you can check that way) then decrement your number

      //and set your destination to 0

      if( SRC == NUM_BOTS) {

        //lcd_putchar('+');

        //lcd_putchar('1');

        //lcd_putchar('+');

        SRC--;

        DEST = 0;

      } else {

        //otherwise decrement both your ID and the destination by 1

        //lcd_putchar('+');

        //lcd_putchar('2');

        //lcd_putchar('+');

        //decrement src and dest

        SRC--;

        DEST--;

      }

    }

    //ensure that the last robot talks to the 0th one

    //this won't be set if the LAST robot dies (all robots are < dead)

    if(SRC == (NUM_BOTS - 1) ) {

      DEST = 0;

    }

    //now your SRC and DEST have been set properly

    //if you are the robot before the dead robot - send a message

    if(SRC == dead_robot) {

      //lcd_putstr(" send packet ");

      //you took the place of the dead robot

      //you start the token ring again

      wl_create_packet();

      wl_send();

    }

    break;

  case WL_ERRCODE_NEEDTOCHARGE:

    break;      //Do nothing about this, yet

  case WL_ERRCODE_CHARGE_ACCEPT:

    // use the value we saved in parse_buffer()

    wl_csloc = max_bom;

    USB_PUTS("\n\rBOM direction: ");

    USB_PUTI(wl_csloc);

    if (wl_sent_cs)

    {

      direction_from_station = wl_buf[WL_DATA_DATA1];

      desired_bay = wl_buf[WL_DATA_DATA2];

      charge_request_register = CHARGE_REQUEST_ACCEPTED;

      USB_PUTS("\n\rReceived charge request response\n\r");

      wl_sent_cs = 0;

      wl_create_packet();

      wl_send();

      orb_set_color(BLUE);

    }

    break;

  case WL_ERRCODE_CHARGE_DENY:

    if (wl_sent_cs)

    {

      //TODO: set flag to tell robot to stop moving, etc...

      charge_request_register = CHARGE_REQUEST_NOT_ACCEPTED;

      usb_puts("\n\rI HAVE BEEN DENIED\n\r");

      wl_sent_cs = 0;

      wl_create_packet();

      wl_send();

      orb2_set_color(RED);

    }

    break;

  case WL_ERRCODE_CHARGE_DEPART://we have sent the charging station a depart request, and now need to continue the token ring

    if (wl_sent_depart)

    {

      usb_puts("Received Departing Ack from Station\n\r");

      wl_sent_depart = 0;

      wl_cs_flag = NOT_REQUEST_CS;

      wl_create_packet();

      wl_send();

    }

    break;

  case WL_ERRCODE_CHARGE_DOCK:

    if (wl_sent_dock)

    {

      usb_puts("Received Docking Ack from Station\n\r");

      wl_sent_dock = 0;

      wl_cs_flag = NOT_REQUEST_CS;

      wl_create_packet();

      wl_send();

    }

    break;

  default:

    //do other stuff;

    break;

  } 

}

/**

   \fn wl_death_handler

   Handles robot death (for the one robot that detects death).

   The robot that currently 'senses' robot death is the robot AFTER the dead

   robot in the ring.  It knows that PREV (the robot right before it) is dead.

   Robot creates a error packet and sends it. 

   This will also call wl_error_handler since wl_buf was set with the current

   dead robot

*/

void wl_death_handler( void ) 

{

  //set time out and death count/flags to 0

  wl_to_count = 0;

  wl_to_flag = 0;

  wl_to_death_flag = 0;

  wl_to_death_count = 0;

  //create a packet with the error

  wl_create_error_packet(WL_ERRCODE_ROBOTDEATH);

  wl_buf[WL_ERR_DEAD_LOC] = PREV;   //PREV is the robot that is dead

  wl_make_checksum();                    //Recompute checksum after adding data

  //send the packet

  wl_send();

  //handle the error yourself

  wl_error_handler(wl_buf[WL_ERROR_LOC]);

  return;

}

/**

   \fun parse_buffer

   Parses the buffer in wl_buf.

   Does nothing if the buffer isn't full.

*/

void parse_buffer () 

{

  //deal with timeout case

  /*

  if( wl_to_flag ) {

    usb_putc('x');

    wl_to_flag = 0;

    wl_create_packet();

    wl_send();

    return;

  }

*/

  while (1)

  {

    if (buffer_size < WL_PKT_LEN)

      return;

    char c;

    RING_BUFFER_REMOVE(buffer, c);

    buffer_size--;

    //if (usb_debug)

    //  usb_putc(c);

    if (c == 'R')

    {

      RING_BUFFER_REMOVE(buffer, c);

      //fprintf(usb_fd, "%x ", c);

      buffer_size--;

      //if (usb_debug)

      //  usb_putc(c);

      if (c == 'C')

        break;

    }

  }

  //wl_to_count = 0;

  wl_index = 2;

  wl_chksum = 0;

  //read bom here, save value for later (error_handler??)

  //max_bom = getMaxBom();

  //get the packet from the ring buffer -- make sure it's valid

  while (wl_index != 0)

  {

    char wl_input;

    RING_BUFFER_REMOVE(buffer, wl_input);

    buffer_size--;

    if( wl_index >= WL_SRC_LOC ) {

      if( wl_index < WL_CHK_LOC ) {

        //getting the rest of the packet

        wl_buf[wl_index] = wl_input;

        wl_chksum += wl_input;

        wl_index++;

      }else{

        //we are at checksum location

        wl_buf[wl_index] = 0;

        wl_chksum += wl_input;

        wl_index = 0;

        USB_PUTC('o');

        if(!wl_chksum) {

          //checksum is correct - equal to zero

          if(set_orb_wireless) {

            //orb2_set_color(GREEN);

          }

          //lcd_putchar('p');

          USB_PUTC('p');

          //get the maximum BOM from the person who sent you the packet

          //max_bom = getMaxBom();

          //lcd_putint(max_bom);

          //lcd_putchar(' ');

          //add to your own sensor the other robot's direction

          //        max_bom  you  them

          //usb_puti(max_bom);

          //usb_putc(' ');

          //addSensor(max_bom, SRC, wl_buf[WL_SRC_LOC], NUM_BOTS);

          //also set whether that robot can see the light or not

          //set_light_status(wl_buf[WL_SRC_LOC], wl_buf[WL_DATA_LIGHT_LOC]);

        }else{  

          //chksum failed

          //usb_putc('F');

          int i;

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

            USB_PUTC(wl_buf[i]);

          }

          USB_PUTS("Fail");

          usb_putstr("Fail");

          return;

          //need to return -- was at the end of the ISR

        }

        break;

      }

    }

  }

  //acknowledge you take the packet by changing the chksum to nonzero

  wl_chksum = 1;

  USB_PUTS("|received_a_packet|");

  if (wl_buf[5] == COLONET_REQUEST || wl_buf[5] == COLONET_COMMAND)

  {

    //orb_set_color(YELLOW);

    //colonet_handle_message(SRC, wl_buf);

    colonet_handle_message(wl_buf);

    return;

  }

  //this is the case when you get your first packet -- you want to join the ring

  if(first_time == 1) {

    USB_PUTS("|first_packet|");

    //you received a first packet from someone

    first_time = 0;

    //get the current number of robots

    NUM_BOTS = wl_buf[NUM_BOTS_LOC];

    //you are now the last robot in the line

    SRC = NUM_BOTS;

    //you are sending back to the first robot

    DEST = 0;

    //since you've just added yourself, increase numbots

    NUM_BOTS++;

    //wl_wait_for_turn = 1

    //figure out a way to send out a packet TODO

    wl_create_packet();

    wl_send();

  }

  if(wl_wait_for_turn) {

    if(NUM_BOTS <= 2 || wl_buf[WL_DEST_LOC] == SRC - 1 ) {

      //you just got a packet from the 'old' last robot send yours

      //lcd_putchar('S');

      wl_wait_for_turn = 0;

      first_time = 1;

      //so we don't wait with a delay

      wl_create_packet();

      wl_send();

      first_time = 0;

      return;

    }else{

      //otherwise just exit out of parse

      return;

    }

  }

  //check for error packet first

  if(wl_buf[WL_DEST_LOC] == WL_PKT_ERROR){

    orb2_set_color(RED);

    //handle error packet

    wl_error_handler( wl_buf[WL_ERROR_LOC] );

    return;

  }

  //see if there are more robots this turn around -- NUM_ROBOTS would have 

  //increased

  if(wl_buf[NUM_BOTS_LOC] > NUM_BOTS){

    //if you are the last robot

    if(DEST == 0){

      //if you are the last robot in the ring

      //change your dest to the new number (the last bot)

      DEST = NUM_BOTS;

    }

    NUM_BOTS = wl_buf[NUM_BOTS_LOC];

  }

  if(wl_buf[WL_SRC_LOC] == SRC) {

    return;

  }

  //increment time out count so we respond first if we are second in line

  int next = wl_buf[WL_DEST_LOC] + 1;

  if (next == NUM_BOTS)

    next = 0;

  if (next == SRC)

    wl_to_count = 1;

  //get the data out of the sensor row

  int i = 0;

  int self = wl_buf[WL_SRC_LOC];

  //13-4 = 9

  int end = MIN(WL_DATA_ROW_END, NUM_BOTS+WL_DATA_ROW_START);

  for(i = WL_DATA_ROW_START; i < end; i++){

    addSensor(wl_buf[i], self, (i-WL_DATA_ROW_START), NUM_BOTS);

  }

  //if you are the destination - respond

  if(wl_buf[WL_DEST_LOC] == SRC) {

    //you know that you got a message addressed to you -- previous robot must 

    //not be dead

    wl_to_death_count = 0;

    //the robot previous to you is in wl_buf[WL_SRC_LOC]

    PREV = wl_buf[WL_SRC_LOC];

    //create a packet

    //lcd_putchar('s');

    USB_PUTS("|received.packet.addr.me|");

    switch (wl_cs_flag)

    {

    case REQUEST_CS:

      wl_sent_cs = 1;

      wl_create_charging_error_packet();

      wl_send();

      usb_puts("\n\rRequested charge\n\r");

      return;

    case DEPART_CS:

      wl_sent_depart = 1;

      wl_create_leaving_packet();

      wl_send();

      //wl_cs_flag = NOT_REQUEST_CS;

      usb_puts("\n\rSent Leaving Packet\n\r");

      return;

    case ARRIVE_CS:

      wl_sent_dock = 1;

      wl_create_docking_packet();

      wl_send();

      //wl_cs_flag = NOT_REQUEST_CS;

      usb_puts("Sent Docking Packet\n\r");

      return;

    }

    wl_create_packet();

    wl_send();

  }

  else

  {

    usb_putstr("packet not addressed to me");

    usb_puti(wl_buf[WL_DEST_LOC]);

  }

  return;

}

/**

   \fun wl_init

*/

void wl_init(void)

{

  wl_index = 0;

  wl_chksum = 1;

  wl_buf[0] = 'R';

  wl_buf[1] = 'C';

  /* executes the function about every 1 sec */

  //rtc_init(HALF_SECOND, &wl_timeout_handler);

  wl_to_flag = 0;

  wl_to_count = 0; 

  wl_to_death_count = 0;

  wl_to_death_flag = 0;

  wl_to_max = WL_TO_INITIAL;

  first_time = 1;

  wl_wait_for_turn = 0;

  wl_sent_cs = 0;

  wl_sent_depart = 0;

  wl_sent_dock = 0;

  UCSR1B |= _BV(RXCIE);

  sei();

}

void wl_send ( void ) {

  int i = 0;

  //turn off interrupt so you don't receive packet as you send it

  UCSR1B &= ~_BV(RXCIE);

  USB_PUTS("\n\r|sending---\n\r");

  if(set_orb_wireless) {

    orb2_set_color(BLUE);

  }

  led_user(1);

  if(SRC == (NUM_BOTS - 1)) {

    //if you are the last robot

    //delay for some time

    if(set_orb_wireless) {

      orb2_set_color(RED);

    }

    delay_ms(LAST_ROBOT_DELAY);

  }

  bom_on();

  delay_ms(WL_DELAY_BOM_PRE);

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

    //print characters to wireless

    xbee_putc(wl_buf[i]);

    //print characters to usb if needed

    //USB_PUTC(wl_buf[i]);

  }

  delay_ms(WL_DELAY_BOM_POST);

  //this is where you would turn the BOM off

  bom_off();

  led_user(0);

  //sprintf(buf, "%d\n\r", wl_buf[WL_CHK_LOC]);

  //put_string(buf);

  //turn it back on so you can get things now

  UCSR1B |= _BV(RXCIE);

}

void wl_create_packet () {

  //Header for Colony Project

  wl_buf[0] = 'R';

  wl_buf[1] = 'C';

  wl_buf[WL_SRC_LOC] = SRC;     //Where the packet came from

  wl_buf[WL_DEST_LOC] = DEST;   //Who the packet is for

  //Behavior Specific Data goes here

  wl_buf[WL_DATA_DATA0] = 0;

  wl_buf[WL_DATA_DATA1] = 0;

  wl_buf[WL_DATA_DATA2] = 0;

  //fill packet with data -- sensor row

  for(int i = WL_DATA_ROW_START; i <= WL_DATA_ROW_END; i++) {

    wl_buf[i] = sensors[SRC][i - WL_DATA_ROW_START];

  }

  //add the number of robots in there

  wl_buf[NUM_BOTS_LOC] = NUM_BOTS;

  //create the checksum

  wl_make_checksum();

}

//Add custom data to the packets

void wl_create_data_packet(unsigned char d0, unsigned char d1, 

                           unsigned char d2)

{

  //Header for Colony Project

  wl_buf[0] = 'R';

  wl_buf[1] = 'C';

  wl_buf[WL_SRC_LOC] = SRC;     //Where the packet came from

  wl_buf[WL_DEST_LOC] = DEST;   //Who the packet is for

  //Behavior Specific Data goes here

  wl_buf[WL_DATA_DATA0] = d0;

  wl_buf[WL_DATA_DATA1] = d1;

  wl_buf[WL_DATA_DATA2] = d2;

  //fill packet with data -- sensor row

  for(int i = WL_DATA_ROW_START; i <= WL_DATA_ROW_END; i++) {

    wl_buf[i] = sensors[SRC][i - WL_DATA_ROW_START];

  }

  //add the number of robots in there

  wl_buf[NUM_BOTS_LOC] = NUM_BOTS;

  //create the checksum

  wl_make_checksum();

}

void wl_create_leaving_packet()

{

  wl_create_error_packet(WL_ERRCODE_CHARGE_DEPART);

}

void wl_create_docking_packet()

{

  wl_create_error_packet(WL_ERRCODE_CHARGE_DOCK);

}

void wl_create_charging_error_packet()

{

  wl_create_error_packet(WL_ERRCODE_NEEDTOCHARGE);

}

//Send an error packet

void wl_create_error_packet(unsigned char errcode)

{

  //Header for Colony Project

  wl_buf[0] = 'R';

  wl_buf[1] = 'C';

  //DEST is all robots

  wl_buf[WL_SRC_LOC] = SRC;

  wl_buf[WL_DEST_LOC] = WL_PKT_ERROR;

  wl_buf[WL_ERROR_LOC] = errcode;

  //rest of the packet is just a bunch of 0s

  for (int i = WL_ERR_DEAD_LOC + 1; i <= WL_DATA_END; i++) {

    wl_buf[i] = 0;

  }

  //add the number of robots in there

  wl_buf[NUM_BOTS_LOC] = NUM_BOTS;

  //create the checksum

  wl_make_checksum();

}

//Computes the checksum

void wl_make_checksum() 

{

  char checksum = 0;

  for(int i = WL_SRC_LOC; i < WL_CHK_LOC; i++) {

    checksum -= wl_buf[i];

  }

  wl_buf[WL_CHK_LOC] = checksum;

}

ISR(USART1_RX_vect) {

  char tmp = UDR1;

  RING_BUFFER_ADD(buffer, tmp);

  buffer_size++;

  //we got a byte, so we're not timing out

  wl_to_count = 0;

  return;

}