Colony

#include "data_response.h"

#include <dragonfly_lib.h>

#include "wl_token_ring.h"

#include "wireless.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);

void data_request_init(void){

  range_init();

  pgh.timeout_handler = NULL;

  pgh.handle_response = NULL;

  pgh.handle_response = handle_receive;

  pgh.unregister = NULL;

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

  char buf[MAX_PACKET_LENGTH]; 

  int size;

  switch(type){

  case BOM_TYPE:

    size = generate_bom_packet(&buf); 

    wl_send_robot_to_robot_packet(DATA_REQUEST_GROUP,BOM_TYPE,data,size,source,0);

  case IR_TYPE:

    size = generate_IR_packet(&buf); 

    wl_send_robot_to_robot_packet(DATA_REQUEST_GROUP,IR_TYPE,data,size,source,0);

  case ENCODER_TYPE:

    //Not yet supported.

  }

}

int generate_bom_packet(char** buf){

  short int i=0, robot_id; 

  char bom_data;

  wl_token_iterator_begin();

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

    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.

  return i;

}

int generate_IR_packet(char** buf){

  *buf[0] = range_read_distance(IR1);   

  *buf[1] = range_read_distance(IR2);   

  *buf[2] = range_read_distance(IR3);   

  *buf[3] = range_read_distance(IR4);   

  *buf[4] = range_read_distance(IR5); 

  return 5;

}