Colony

/** @file robot_route_reg_test.c

 * @author Eugene Marinelli

 * @brief Tests function registration in the colonet.h robot library

 */

#include <firefly+_lib.h>

#include <colonet_robot/colonet.h>

#include <serial.h>

#include "wl_adhoc.h"

void f0_handler(void)

{

  static int color = 0;

  color = !color;

  if (color) {

    orb_set(0,255,0);

  } else {

    orb_set(255,0,255);

  }

  buzzer_init();

  delay_ms(200);

  buzzer_off();

}

int main(void) {  

  analog_init();

  led_init();

  orb_init();

  motors_init();

  buzzer_init();

  lcd_init();

  wl_init();

  serial_init(BAUD115200);

  serial1_init(BAUD115200);

  fdevopen(&serial1_putchar, &serial1_getchar);

  orb_set(255,0,0);

  colonet_add_message(0xF0, f0_handler);

  orb_set(0,0,255);

  while(1){

    parse_buffer();

  }

  return 0;

}