Colony

/**

   \file wl_adhoc.c

   Wireless ad hoc networking.

   NOW WITH SENSORS

   Robots can come in and out of the ring.

*/

/*

  Wireless ad hoc networking + sensors

  If a robot is the first to turn on, it will set itself as the leader

  The next robot will receive that packet and tokenize with it.

  Each other robot turned on will receive packets, and join the ring.

  All the robots will also change their network topology (num_bots)

  2006-11-21: Felix

              Fixed buzzer.c code (copied from a different dir).

              Code still works on robots at this time 

              Felix used this code to figure out the robot resetting problem.  BOM sensor 10

              needs to be taped up.

              You may put this event in the history books.

  2006-10-31 Kevin

  Changed the boards to the 115200 Baud rate. Changed the pound define to reflect this.

  The boards have been hardcoded to do this. They are labelled in case you do not know

  which ones do this. It works.

  2006-10-13: Kevin

  Made improvements to make the module more modular and general

  so that you can just drag and drop it into any behavior.

  Began the integration of autonomous charging. We're using the

  error system that is built into wl_adhoc originally to ping the

  charge stations. Do not know if it works, can't test it yet. But

  wireless normally still works.

  2006-08-07: Felix

  Added the use of the sensor row that was being transmitted

  Robots now share a common table of sensor 'positions'

  Just add light sensing and you're good to go

  WARNING: Robots seem to reset randomly due to batteries, especially

  when moving around.  Could be brownout detector kicking in.

  Added a beep at startup to detect reset.

  2006-08-03: Felix

  Now added follow-the-leader code

  Battery problem -- testing only really worked on power supply

  Robots follow the leader (theoretically).

  They fill their own row of the sensors matrix, but do not get

  the other robots' row -- not in the code.

  2006-07-27: Felix

  Created new directory for sensors use

  2006-07-24: Felix

  Added BOM.c/BOM.h

  bom_test works -- gets max bom

  Added the get_maxbom each time you get a wireless packet

  ---NEEDS TESTING

  Also, packet sent should contain your 'row'

  Also, received packet should be stored in matrix

  2006-07-13: Felix

  Works for the most part

  W00t

  Still a few kinds I imagine, but robot death & birth work

*/

#include "firefly+_lib.h"

#include "wl_adhoc.h"

#include <avr/interrupt.h>

#include <colonet.h>

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

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

unsigned char set_orb_wireless = 0;

//unsigned char set_orb_wireless = 1;

/*

  int main( void ) 

  {

  wl_adhoc();

  bom_test();

  return 0;

  }

*/

void bom_test (void)

{

  int maxb;

  lcd_init();

  analog_init();

  while(1) {

    if(button1_read()){

      lcd_clear_screen();

      bom_on();

      delay_ms(100);

    }

    if(button2_read()) {

      lcd_clear_screen();

      bom_off();

      delay_ms(100);

    }

    maxb = getMaxBom();

    lcd_putint(maxb);

    delay_ms(50); 

  }

}

void wl_adhoc()

{

  //initialize the serial modules

  serial_init(WL_BAUD);       //XBee

  serial1_init(BAUD115200);   //Serial port

  //lcd, orb, all that good stuff

  lcd_init();

  analog_init();

  led_init();

  orb_init();

  motors_init();

  buzzer_init();

  //init the wireless

  wl_init();

  //int max_bom_to_follow;

  while(1){

    //this needs to happen continuously -- it handles the packet

    parse_buffer();

    /*

      if(SRC ==  0) {

      //leader -- just drive

      drive_forward();

      }

      else {

      max_bom_to_follow = getSelfSensor(SRC, PREV, NUM_BOTS);

      //lcd_putchar('f');

      //lcd_putchar(max_bom_to_follow + 48);

      //lcd_putchar(' ');

      seek(max_bom_to_follow);

      }

    */

    //button1 prints debug information

    if(button1_read()) {

      //lcd_clear_screen();

      display_debug();

      delay_ms(10);

    }

    /*

    //button2 does nothing

    if(button2_read()) {

      //wl_create_packet();

      //wl_send();

      //delay_ms(10);

    }

    */

  }

  return;        

}

/**

   \fn display_debug

   \brief displays debug information

   Prints out 4 numbers, in the following order

   ( PREV | SRC | DEST | NUM_BOTS | wl_to_death_count )

   where PREV is the previous robot before you

   SRC is you

   DEST is your destination

   NUM_BOTS is the total number of robots in the ring

   wl_do_death_count is the number of timeouts that will result in a death

*/

void display_debug()

{

  lcd_clear_screen();

  lcd_putchar('(');

  lcd_putchar(PREV+48);

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

  //print table

  int y = 1;

  lcd_gotoxy(0, y);

  int i, j;

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

    for(j = 0; j < NUM_BOTS; j++) {

      lcd_putchar( getSelfSensor(i, j, NUM_BOTS) + 48);

      lcd_putchar(' ');

    }

    y++;

    lcd_gotoxy(0, y);

  }

}

/*

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

  //increment the total number of wireless time-outs

  wl_to_count++;

  //lcd_putchar('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 you have enough timeouts

    if(wl_to_count > WL_TIMEOUT){

      //lcd_putchar('z');

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

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

{

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

  switch(errcode) {

  case WL_ERRCODE_ROBOTDEATH:

    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:

    if (CHARGESTATION == 1) {

      /*Turn on BOM for the length of time robots normally would have their 

        BOM on. This is effectively telling everyone where the charging 

        station is.  Assuming only one charging station at the moment.

      */

      bom_on();

      delay_ms(WL_DELAY_BOM_PRE);

      wl_create_error_packet(WL_ERRCODE_CHARGESTATION);

      wl_send();

      delay_ms(WL_DELAY_BOM_POST);

      bom_off();

    }

    break;      //Do nothing about this, yet

  case WL_ERRCODE_CHARGESTATION:

    // If the Charging Station responds, look for where it's comming from and 

    //store that value for later

    //wl_csloc = getMaxBOM();

    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(void) 

{

  //deal with timeout case

  if( wl_to_flag ) {

    wl_to_flag = 0;

    wl_create_packet();

    wl_send();

    return;

  }

  //do nothing is packet is not complete

  if( (wl_chksum != 0) || (wl_index != 0) ) {

    return;

  }

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

  wl_chksum = 1;

  if(wl_buf[WL_DATA_DATA1] == COLONET_REQUEST || 

     wl_buf[WL_DATA_DATA1] == COLONET_COMMAND){

    printf("colonet message received: code %d\n", wl_buf[WL_DATA_DATA2]);

    colonet_handle_message(SRC, wl_buf);

  }else{

    printf("\ndata1:%d\n", wl_buf[WL_DATA_DATA1]);

  }

  if(first_time == 1) {

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

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

    //wl_wait_for_turn = 1;

    //create and send a packet

    wl_create_packet();

    wl_send();

  }

  //IS NOT CALLED FOR NOW

  if(wl_wait_for_turn) {

    if( wl_buf[WL_DEST_LOC] == (NUM_BOTS - 1) ) {

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

      //lcd_putchar('S');

      wl_create_packet();

      wl_send();

    }else{

      //otherwise just exit out of parse

      return;

    }

  }

  //check for error packet first

  if(wl_buf[WL_DEST_LOC] == WL_PKT_ERROR){

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

      //lcd_putchar('j');

    }

    //increase the number of robots to what it is in the incoming packet

    NUM_BOTS = wl_buf[NUM_BOTS_LOC];

  }

  //make sure packet isn't your own (shouldn't really happen)

  if(wl_buf[WL_SRC_LOC] == SRC) {

    //lcd_putchar('d');

    return;

  }

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

  //lcd_putchar('s');

  //lcd_putchar(self+48);

  //lcd_putchar(':');

  //lcd_putchar(end+48);

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

    wl_create_packet();

    wl_send();

  }

  return;

}

/**

   \fun wl_init

*/

void wl_init(void)

{

  lcd_putchar('~');

  lcd_putchar(SRC+48);

  lcd_putchar('~');

  lcd_putchar(DEST+48);

  lcd_putchar('~');

  lcd_putchar(NUM_BOTS+48);

  lcd_putchar('~');

  buzzer_init();

  buzzer_set_freq(C5);

  delay_ms(800);

  buzzer_off();

  wl_index = 0;

  wl_chksum = 1;

  wl_buf[0] = 'R';

  wl_buf[1] = 'C';

  /* executes the function about every 1/4 sec */

  rtc_init(PRESCALE_DIV_128, 64, &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;

  UCSR0B |= _BV(RXCIE) | _BV(RXEN);

  sei();

}

void wl_send ( void ) {

  int i = 0;

  if(set_orb_wireless) {

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

      orb_set_color(RED);

    }

    delay_ms(LAST_ROBOT_DELAY);

    //warning: delay is actually 0 -- adding the delay back causes issues.

  }

  //this is where you would turn on the BOM

  bom_on();

  delay_ms(WL_DELAY_BOM_PRE);

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

    serial_putchar(wl_buf[i]);

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

}

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

}

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

}

//use on new avr-libc

//ISR(USART0_RX_vect)

SIGNAL(SIG_USART0_RECV)

{

  int max_bom;

  wl_input = UDR0;

  //printf("%d ", wl_input);

  if((wl_index == 0) && (wl_chksum != 0)){

    //first packet? - check chksum to make sure we're not beginning

    if( wl_input == 'R' ) {

      //got the first code

      wl_index++;

      //if(set_orb_wireless) {

      //  orb_set_color(GREEN);

      //}

      //lcd_putchar('R');

      //serial1_putchar('R');

      wl_to_count = 0;                   //at this point we're not timing out, reset counter

    }

    return;

  }else if( wl_index == 1 ){

    if(wl_input == 'C') {

      wl_index++;

      wl_chksum = 0;

      //lcd_putchar('C');

      //serial1_putchar('C');

    }else{

      //incomplete packet header

      wl_index = 0;

    }

    return;

  }

  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;

      if(!wl_chksum) {

        //checksum is correct - equal to zero

        //if(set_orb_wireless) {

        //  orb_set_color(ORB_OFF);

        //}

        //lcd_putchar('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

        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

        //serial1_putchar('F');

        //lcd_putchar('F');

      }

    }

  }

}