Colony

#include "dragonfly_lib.h"

#include "wireless.h"

#include "wl_token_ring.h"

#include "smart_run_around_fsm.h"

#include "slam_defs.h"

#define BOM_POST_DELAY 50

#define PACKET_BUFFER_SIZE 101 

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

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

}

//These can probably be NULL..

void timeout_handle(void){

}

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

    /*Send a message back to our computer overlord.*/

    char buf[PACKET_BUFFER_SIZE]; 

    int len = make_request_packet(&buf);

    char packet[len];

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

      packet[i]=buf[i];

    } 

    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++){

    }

  }

}

/**

Packets are structured {IR1, IR2, IR3, IR4, IR5,

BomID1, ... , BomValue1, BomID2, ... , BomValue2, etc.} 

Total size is 5 + 3*numRobots. So Xbee should support

up to 31 robots before we need to restructure the 

communication between the computer and the robot.

*/

int make_request_packet(char** packet){

  int i,j;

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

      (*packet)[i] = IR[i];

  } 

  i=5; 

  wl_token_iterator_begin();

  int id = wl_get_xbee_id();  

  while(wl_token_iterator_has_next()){

    int robot_in_view = wl_token_iterator_next(); 

    //Robot int's are two bytes. 

    (*packet)[i] = robot_in_view; 

    i+=2;

    int bom_number = wl_token_get_sensor_reading(id,robot_in_view);

    //Bom reading of 255 denotes an unseen node. 

    if(bom_number<0) bom_number = 255; 

    bom_number %= 256;

    char bom_value = (char) bom_number;

    (*packet)[i] = bom_value;

    i++;

  }

  //Size is one bom byte towards every robot, and 5 IR bytes. 

  return wl_token_get_num_robots()+5;

}