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, i;

  usb_puts("Got a packet!\n\r");

  switch(type){

  case BOM_TYPE:

    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) & 255);

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

      //TODO Ensure that wl_token_get.... returns the expected char.

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

  case IR_TYPE:

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

    }

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

  case ENCODER_TYPE:

    break;

    //Not yet supported.

  }

}

/**

* @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;

}