Colony

#include "dragonfly_lib.h"

#include "data_response.h"

#include "wl_token_ring.h"

#include "wireless.h"

#include "serial.h"

PacketGroupHandler pgh;

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

int generate_bom_packet(char** buf);

int generate_IR_packet(char** buf);

/** 

* @brief The only function that should be called by user code.

  Prepares the robot to receive and respond to data requests.

*/

void data_response_init(void){

  //usb_init();

  //usb_puts("USB initialized!\n\r");

  range_init();

  pgh.groupCode=DATA_REQUEST_GROUP; 

  pgh.timeout_handler = NULL;

  pgh.handle_response = NULL;

  pgh.handle_receive = handle_receive;

  pgh.unregister = NULL;

  wl_register_packet_group(&pgh);

  //usb_puts("Packet Group Registered!\n\r");

}

/**

* Sorry for the lack of modularity, I didn't want to deal with memory issues for too long, plus it'll be faster!

*/

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

  char buf[MAX_PACKET_LENGTH]; 

  int size;

  int i=0;

  short int robot_id, ir_value; 

  char ranges[] = {IR1,IR2,IR3,IR4,IR5}; 

  switch(type){

  case BOM_TYPE:

    wl_token_iterator_begin();

    while(wl_token_iterator_has_next() && i<=96){

      robot_id = wl_token_iterator_next();

      buf[i++] = (char)((robot_id>>8) & 255);

      buf[i++] = (char)(robot_id & 255);

      buf[i++] = (char)(wl_token_get_my_sensor_reading(robot_id));

    }

    //i is one greater than the index of the last byte... the size.

    wl_send_robot_to_robot_global_packet(DATA_REQUEST_GROUP,BOM_TYPE,buf,i,source,0);

    break;

  case IR_TYPE:

    //usb_puts("Generating an IR packet.\n");

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

      ir_value = (short)range_read_distance(ranges[i]);

      buf[2*i] = (char)((ir_value>>8)&255);   

      buf[2*i+1] = (ir_value&255);   

      //usb_puts("Distance :\t");

      //usb_puti(ir_value);

      //usb_puts("\n");

    }

    size = 10;

    //usb_puts("Packet Contents: ");

    /*for(i = 0; i<size;i++){

        //usb_puti(buf[i]);

        usb_putc(' ');

    }*/

    wl_send_robot_to_robot_global_packet(DATA_REQUEST_GROUP,IR_TYPE,buf,size,source,0);

    break; 

  case ENCODER_TYPE:

    break; 

    //Not yet supported.

  }

  case ALL_TYPE:

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

      ir_value = (short)range_read_distance(ranges[i]);

      buf[2*i] = (char)((ir_value>>8)&255);   

      buf[2*i+1] = (ir_value&255);   

    } //Buf 0...9 filled.

    //Tack on the Encoder data... 

    //Then BOM: 

    wl_token_iterator_begin();

    while(wl_token_iterator_has_next() && i<=96){

      robot_id = wl_token_iterator_next();

      buf[i++] = (char)((robot_id>>8) & 255);

      buf[i++] = (char)(robot_id & 255);

      buf[i++] = (char)(wl_token_get_my_sensor_reading(robot_id));

    }

    wl_send_robot_to_robot_global_packet(DATA_REQUEST_GROUP,ALL_TYPE,buf,size,source,0);

    break;

}

/** 

* @brief  Generates a BOM Data packet. **NO LONGER USED** Delete once this code is used often.

*/

int generate_bom_packet(char** buf){

  short int i=0, robot_id; 

  wl_token_iterator_begin();

  while(wl_token_iterator_has_next() && i<=96){

    robot_id = wl_token_iterator_next();

    (*buf)[i++] = (char)((robot_id>>8) & 0xFF);

    (*buf)[i++] = (char)(robot_id & 0xFF);

    (*buf)[i++] = (char)(wl_token_get_my_sensor_reading(robot_id));

  }

  //i is one greater than the index of the last byte... the size.

  return i;

}

/** 

* @brief Generates an IR Packet **NO LONGER USED** Delete once this code is used often.

* 

*/

int generate_IR_packet(char** buf){

  //usb_puts("Generating an IR packet.\n");

  char ranges[] = {IR1,IR2,IR3,IR4,IR5}; 

  int i;

  short ir_value; 

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

    ir_value = (short)range_read_distance(ranges[i]);

    (*buf)[2*i] = (char)((ir_value>>8)&255);   

    (*buf)[2*i+1] = (ir_value&255);   

    //usb_puts("Distance :\t");

    //usb_puti(ir_value);

    //usb_puts("\n");

  }

  return 10;

}