Colony

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

      }

    }

  }

}