Colony

#include <stdlib.h>

#include <stdio.h>

#include <unistd.h>

#include "data_requests.h"

#include "wireless.h"

#include "wl_token_ring.h"

#include "pthread.h"

void IR_handler(unsigned char* data);

void bom_handler(BomNode* head);

void encoder_handler(unsigned char* data);

unsigned char bom_responded_flag;

unsigned char ir_responded_flag;

unsigned char encoder_responded_flag;

void* wl_do_routine(void* arg){

  while(1){

     wl_do();

  }

}

pthread_t* wl_do_thread;

int main(void){

  wl_set_com_port("/dev/cu.usbserial-A1000Qu6");

  wl_init();

  wl_token_ring_register();

  data_requests_init(bom_handler,IR_handler,encoder_handler);

  int robot_id;

  bom_responded_flag = 0;

  ir_responded_flag = 0;

  encoder_responded_flag = 0;

  int test_count; 

  wl_do_thread = malloc(sizeof(pthread_t));

  pthread_create(wl_do_thread,NULL,wl_do_routine,NULL);

  while(1){

      printf(".");

      wl_token_iterator_begin();

      while(wl_token_iterator_has_next()){

        robot_id = wl_token_iterator_next();

        bom_responded_flag = 0;

        ir_responded_flag = 0;

        encoder_responded_flag = 0;

/*

        request_bom_data(robot_id);

        while(!bom_responded_flag){

          delay_ms(50);

        }

*/       

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

        request_IR_data(robot_id);

        test_count=0;

        while(!(ir_responded_flag)&&(test_count++<20)){

          printf("Waiting on %i..\n",robot_id);

          usleep(5000000);

        }

/*       

        request_encoder_data(robot_id);

        while(!encoder_responded_flag){

          delay_ms(50);

        }

*/

      }

      usleep(5000000);

  }

}

void IR_handler(unsigned char* data){

  char i = 0;

  printf("IR Callback occurred.\n");

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

    printf("IR %i is %i\n",i+1,(int)data[i]);

  }

  ir_responded_flag=1;

}

void bom_handler(BomNode* head){

  bom_responded_flag=1;

}

void encoder_handler(unsigned char* data){

  encoder_responded_flag=1;

}