Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include <wl_token_ring.h>

#include <smart_run_around_fsm.h>

#define BOM_POST_DELAY 50

#define SLAM_PACKET_GROUP 11

#define CONTROL_PACKET_TYPE 'C'

#define REQUEST_PACKET_TYPE 'R'

#define ROBOT_RESPONSE 'I'

//REQUEST_PACKET_ORGANIZATION

#define REQUEST_PACKET_SIZE 50

#define IR_1_LOC 0

#define IR_2_LOC 1

#define IR_3_LOC 2

#define IR_4_LOC 3

#define IR_5_LOC 4

#define BOM_START_LOC 5

void bom_pre(void);

void bom_post(void);

//Packet Handler Functions

void timeout_handle(void);

void response_handle(int frame, int received);

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

void unregister(void);

char* make_request_packet(char* packet);

int IR[5];

int main(void)

{

  dragonfly_init(ALL_ON);

  wl_init();

  wl_token_ring_register();

  wl_token_ring_join();

  wl_token_ring_set_bom_functions(bom_pre, bom_post, get_max_bom);

  PacketGroupHandler packet_group_handler;

  packet_group_handler.groupCode=SLAM_PACKET_GROUP;

  packet_group_handler.timeout_handler=timeout_handle;

  packet_group_handler.handle_response=response_handle;

  packet_group_handler.handle_receive=receive_handle;

  packet_group_handler.unregister=unregister;

  wl_register_packet_group(&packet_group_handler);

  int i;

  while(1){

    wl_do();

    run_around_FSM(); 

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

      IR[i]=range_read_distance(i);

    }

  }

  wl_unregister_packet_group(&packet_group_handler); 

}

void bom_pre(void){

    bom_on();

}

void bom_post(void){

    delay_ms(BOM_POST_DELAY);

    bom_off();

}

void timeout_handle(void){

    wl_token_ring_join();

}

void response_handle(int frame, int received){

}

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

  int i; 

  if(type==REQUEST_PACKET_TYPE){

    char packet[REQUEST_PACKET_SIZE]; 

    //Send a message back to our computer overlord.

    int len= make_request_packet(packet);

    wl_send_robot_to_robot_packet(SLAM_PACKET_GROUP, ROBOT_RESPONSE, packet, len, source , 0);

  }

  else if(type == CONTROL_PACKET_TYPE){

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

    }

  }

}

int make_request_packet(char* packet){

  int size = wl_token_get_num_robots();

  int i,j;

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

      packet[i] = IR[i];

  } 

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

      for(j=0;j<size;j++){

       // wl_token_get_sensor_reading(int source, int dest);

      }

  }

  return size;

}