Colony

/*

Ryan Kellogg

10/12/06

manualControlRobot - Receives commands from server and executes

*/

/* Includes */

#include <stdio.h>

#include <string.h>

#include <firefly+_lib.h>

#include "pindefs_ff.h"

#include <wireless.h>

#define FORWARD 0xF0

#define BACK 0xF1

#define RIGHT 0xF2

#define LEFT 0xF3

#define STOP 0xF4

void init_hardware(void);

/* Main */

int main(void){

  char buf[80];

  char src, dest;

  init_hardware();

  wl_init(3,1);

  orb_set_color(GREEN);

  while(1){

    if(wl_recv(buf, &src, &dest)){

      lcd_putchar('[');

      lcd_putchar(buf[0]);

      lcd_putchar(']');

      orb_set_color(RED);

    }else{

      printf(".");

      orb_set_color(GREEN);

    }

    switch(buf[0]){

      case FORWARD:

        motor1_set(0,255);

        motor2_set(0,255);

        break;

      case BACK:

        motor1_set(1,255);

        motor2_set(1,255);

        break;

      case LEFT:

        motor1_set(1,255);

        motor2_set(0,255);

        delay_ms(150);

        motors_off();

        break;

      case RIGHT:

        motor1_set(0,255);

        motor2_set(1,255);

        delay_ms(150);

        motors_off();

        break;

      case STOP:

        motors_off();

      }

      buf[0] = 0;

  }

  return 0;

}

void init_hardware(){

  motors_init();

  orb_init();

  led_init();

  analog_init();

  serial_init(BAUD9600);

  serial1_init(BAUD115200);2

  lcd_init();

  fdevopen(&serial1_putchar, &serial1_getchar);

}