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

}

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:

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

    }

    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;

  }

}