Colony

#include "dragonfly_lib.h"

#include "../../libwireless/lib/wireless.h"

#include "../../libwireless/lib/wl_token_ring.h"

#include "smart_run_around_fsm.h"

#include "../slam_defs.h"

#define BOM_POST_DELAY 0

#define PACKET_BUFFER_SIZE 101 

//Packet Handler Functions

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

void unregister(void);

int make_request_packet(char** packet);

void send_response_packet(int dest);

unsigned int response_flag = 0; //Will represent the robot to respond to or 0 for no robot.

char control_flag=0;             //Non zero when the robot is to be controlled.

int IR[5];

int main(void)

{

  dragonfly_init(ALL_ON);

  usb_init();

  usb_puts("USB initialized\n\n\r");

  wl_init();

  wl_token_ring_register();

  wl_token_ring_join();

  PacketGroupHandler packet_group_handler;

  packet_group_handler.groupCode=SLAM_PACKET_GROUP;

  packet_group_handler.timeout_handler=NULL;

  packet_group_handler.handle_response=NULL;

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

    //Uncomment this if you want the robot to do something. 

    //run_around_FSM(); 

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

      IR[i]=range_read_distance(i);

    }

    if(response_flag){

      send_response_packet(response_flag);

      response_flag=0;

    }

    if(control_flag){

      //FILL IN HERE

    }

  }

  wl_unregister_packet_group(&packet_group_handler); 

}

void send_response_packet(int dest){

    usb_puts("Sending a response packet to ");

    usb_puti(dest);

    usb_puts(".\n\r");

    char buf[PACKET_BUFFER_SIZE]; 

    int len = make_request_packet((char**)&buf), i;

    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, dest, 0);

}

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

  if(type==REQUEST_PACKET_TYPE){

    usb_puts("Received a request packet\n\r");

    response_flag = source;

  }

  //Not yet implemented:

  else if(type == CONTROL_PACKET_TYPE){

    control_flag = 1;

    //Put packet data into some global.

  }

}

/**

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;

  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;

}

void unregister(void){

  return;

}