Colony

   1               		.file	"monitor_test.c"

   2               		.arch atmega128

   3               	__SREG__ = 0x3f

   4               	__SP_H__ = 0x3e

   5               	__SP_L__ = 0x3d

   6               	__tmp_reg__ = 0

   7               	__zero_reg__ = 1

   8               		.global __do_copy_data

   9               		.global __do_clear_bss

  10               		.text

  11               	.global	main

  13               	main:

  14               	/* prologue: frame size=0 */

  15 0000 C0E0      		ldi r28,lo8(__stack - 0)

  16 0002 D0E0      		ldi r29,hi8(__stack - 0)

  17 0004 DEBF      		out __SP_H__,r29

  18 0006 CDBF      		out __SP_L__,r28

  19               	/* prologue end (size=4) */

  20               	/* epilogue: frame size=0 */

  21 0008 0C94 0000 		jmp exit

  22               	/* epilogue end (size=2) */

  23               	/* function main size 6 (0) */

  25               	/* File "monitor_test.c": code    6 = 0x0006 (   0), prologues   4, epilogues   2 */

DEFINED SYMBOLS

                            *ABS*:00000000 monitor_test.c

/var/tmp//cc3WAcUk.s:3      *ABS*:0000003f __SREG__

/var/tmp//cc3WAcUk.s:4      *ABS*:0000003e __SP_H__

/var/tmp//cc3WAcUk.s:5      *ABS*:0000003d __SP_L__

/var/tmp//cc3WAcUk.s:6      *ABS*:00000000 __tmp_reg__

/var/tmp//cc3WAcUk.s:7      *ABS*:00000001 __zero_reg__

/var/tmp//cc3WAcUk.s:13     .text:00000000 main

UNDEFINED SYMBOLS

__do_copy_data

__do_clear_bss

__stack

exit

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

}

#include "slam_analyzer.h"

#include <stdio.h>

#include <stdlib.h>

void slam_analyze(RobotData* head){

  printf("Analyzing..\n");

}

void slam_print_data(RobotData* head){

  if(head==NULL) printf("No data to display.\n");

  RobotData* iter = head;

  BomNode* bomIter;

  char i;

  while(iter!= NULL){

    bomIter = iter->bom_head;

    printf("Robot %x:\n",iter->id);

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

      printf("\tSharp IR %i : %i\n\n",i,iter->IR[i]);

    if(BomIter == NULL){

        printf("No Bom Data to Display\n\n\n");

    }

    else{

      while(bomIter!=NULL){

        printf("\tBom reading for robot %x : %i",BomIter->id, BomIter->value);

        bomIter = bomIter->next;

      }

    }

  }

}

#include "../slam_defs.h"

void slam_analyze(RobotData* head);

void slam_print_data(RobotData* head);

#include <stdio.h>

#include <stdlib.h>

#include <unistd.h>

#include <pthread.h>

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

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

#include "slam_analyzer.h"

//Currently request information 2 times a second.

#define SLAM_REQUEST_DELAY 500000

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

void unregister(void);

void slam_request_thread(void*);

RobotData* getRobot(int id);

RobotData* head_bot;

RobotData* current_bot;

int main(void)

{

  PacketGroupHandler* pgh = malloc(sizeof(PacketGroupHandler)); 

  pgh->groupCode = SLAM_PACKET_GROUP;

  pgh->timeout_handler = NULL;

  pgh->handle_response = NULL;

  pgh->handle_receive = handle_receive;

  pgh->unregister = unregister;

  wl_init();

  wl_register_packet_group(pgh);

  wl_token_ring_register();

  //wl_token_ring_join();

  printf("Computer SLAM initialized\n\n");

  //Run the requesting thread simultaneously.

  pthread_t request_thread;

  pthread_create(&request_thread,NULL,slam_request_thread,NULL);

  while(1){

      wl_do(); //Receive packets (responses to Request packets.)

  }

	return 0;

}

//Blocking thread.

void slam_request_thread(void* asdf){

    //int count = 0;

    while(1){

      wl_token_iterator_begin();

      while(wl_token_iterator_has_next()){

        int next = wl_token_iterator_next();

        printf("Sending a request packet to %i\n",next);

        wl_send_robot_to_robot_packet(SLAM_PACKET_GROUP,REQUEST_PACKET_TYPE,

          NULL,0,next,0);  

      }

      usleep(50000);

    }

    usleep(50000);

}

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

  printf("Received a packet\n");

  if(type == ROBOT_RESPONSE){

    int i=0; 

    current_bot = getRobot(source);

    if(current_bot == NULL){

      //Add the new robot to the front of the list.

      current_bot = malloc(sizeof(RobotData));

      current_bot->next = head_bot; //Works even if head_bot is null.

      head_bot = current_bot;

    }

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

      current_bot->IR[i] = packet[i];

      printf("*");

    }

    i++; //i=5 

    //Bom Handling.

    //Clear the current information.

    BomNode* head = current_bot->bom_head;

    if(head!=NULL){

      BomNode* next = head->next;

      while(next!=NULL){

        free(head);

        head = next;

        next = head->next;

      }

      free(head);

    }

    //All of the memory allocated for the BomList is free.

    //The remainder of the packet is bom data. 

    BomNode* bom_node;

    current_bot->bom_head = bom_node;

    //i still points to the first bom information spot at 5. 

    while(i<length){

      printf("*"); 

      bom_node = malloc(sizeof(BomNode));

      bom_node->id = ((short int)((short)packet[i++]<<8) + packet[i++]);

      if(i>=length){

        printf("Packet not constructed correctly. Terminate. \n");

        exit(0);

      }

      bom_node->value = packet[i++];

      if(i>=length){

        printf("Packet not constructed correctly. Terminate. \n");

        exit(0);

      }

      bom_node = bom_node->next;

    }

  }

}

RobotData* getRobot(int id){

  RobotData* next = head_bot; 

  while(next->id != id && next->next!=NULL);

  if(next->id != id) return NULL;

  else return next;

}

void unregister(void){

  return;

}