Colony

/** @file colonet_wireless_test2.cpp

 * @brief colonet_wireless_test2 - tests the colonet_wireless library with

 * manual control.

 *

 * @author Eugene Marinelli

 * @date 2/5/06

 */

#include <iostream>

#include <signal.h>

#include <fcntl.h>

#include <signal.h>

#include "colonet_wireless.h"

#include "colonet_defs.h"

ColonetWireless* cw;

int msg_handler(ColonetPacket* pkt)

{

  printf("Handler!\n");

  return 0;

}

char get_mouse()

{

  int fd;

  char buf[10];

  fd = open("/dev/input/mouse0", O_RDONLY);

  read(fd, buf, 1);

  close(fd);

  return buf[0];

}

void exit_handler(int sig)

{

  unsigned char data[10];

  printf("Sending kill packet...\n");

  cw->send(GLOBAL_DEST, COLONET_COMMAND, MOTORS_OFF, data);

  printf("Exiting...\n");

  sleep(2);

  exit(0);

}

int main()

{

  unsigned char data[10];

  char dir;

  int paused = 0;

  unsigned char motor_back_args[6] = {0, 0, 0, 0xFF, 0};

  unsigned char motor_forward_args[6] = {0, 1, 0, 0xFF, 0};

  cw = new ColonetWireless(SERIAL_PORT, msg_handler, NULL, true, true);

  signal(SIGINT, exit_handler);

  cw->run_listener_thread();

  while(1){

    dir = get_mouse();

    usleep(300000);

    printf("%d\n", dir);

    switch (dir) {

    case 40:

      /* Forward */

      cw->send(GLOBAL_DEST, COLONET_COMMAND, MOTOR1_SET, motor_forward_args);

      cw->send(GLOBAL_DEST, COLONET_COMMAND, MOTOR2_SET, motor_forward_args);

      break;

    case 8:

      /* Back */

      cw->send(GLOBAL_DEST, COLONET_COMMAND, MOTOR1_SET, motor_back_args);

      cw->send(GLOBAL_DEST, COLONET_COMMAND, MOTOR2_SET, motor_back_args);

      break;

    case 24:

      /* Left */

      //cw->send(GLOBAL_DEST, COLONET_COMMAND, MOTORS_OFF, data);

      cw->send(GLOBAL_DEST, COLONET_COMMAND, MOTOR1_SET, motor_forward_args);

      break;

    case 56:

      /* Right */

      //cw->send(GLOBAL_DEST, COLONET_COMMAND, MOTORS_OFF, data);

      cw->send(GLOBAL_DEST, COLONET_COMMAND, MOTOR2_SET, motor_forward_args);

      break;

    case 9:

      /* Left click */

      /* Stop */

      cw->clear_message_queue();

      cw->send(GLOBAL_DEST, COLONET_COMMAND, MOTORS_OFF, data);

      break;

    case 10:

      /* Right click */

      /* Execute first user-defined function */

      cw->send(GLOBAL_DEST, COLONET_COMMAND, 0xF0, data);

      break;

    }

  }

  return 0;

}