Colony

#include "data_response.h"

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

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;

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

  switch(type){

  case BOM_TYPE:

    size = generate_bom_packet(&buf); 

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

  case IR_TYPE:

    size = generate_IR_packet(&buf); 

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

  case ENCODER_TYPE:

    break; 

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

}

int generate_IR_packet(char** buf){

  *buf[0] = range_read_distance(IR1);   

  delay_ms(20);

  *buf[1] = range_read_distance(IR2);   

  delay_ms(20);

  *buf[2] = range_read_distance(IR3);   

  delay_ms(20);

  *buf[3] = range_read_distance(IR4);   

  delay_ms(20);

  *buf[4] = range_read_distance(IR5); 

  char i;

  return 5;

}