Colony

#include <stdio.h>

#include <stdlib.h>

#ifndef ROBOT

#include <fcntl.h>

#include <unistd.h>

#include <pthread.h>

#include <errno.h>

#include <termios.h>

#endif

#define XBEE_GET_PACKET_TIMEOUT 1000

#ifndef ROBOT

static int xbee_read(char* buf, int size);

#endif

#ifndef ROBOT

static char* xbee_com_port = XBEE_PORT_DEFAULT;

static int xbee_stream;

static pthread_t* xbee_listen_thread;

#endif

#ifndef ROBOT

// Computer code

/**

 * Thread that listens to the xbee.

 **/

static void* listen_to_xbee(void* x)

{

	char c;

	while (1)

	{

		if (xbee_read(&c, 1) != 0) {

			WL_DEBUG_PRINT("xbee_read failed.\n");

			return NULL;

		}

		arrival_buf[buffer_last] = c;

		int t = buffer_last + 1;

		if (t == XBEE_BUFFER_SIZE)

			t = 0;

		if (t == buffer_first)

		{

			WL_DEBUG_PRINT("Out of space in buffer.\n");

		}

		buffer_last = t;

		usleep(1000);

	}

	return NULL;

}

#endif

/**

 * Initializes the XBee library so that other functions may be used.

 **/

int8_t xbee_lib_init()

{

  // do serial.c xbee init     TODO: merge this into xbee.c

  if (xbee_init() != 0) {

    usb_puts("xbee_init error");

    return WL_ERROR_INIT_FAILED;

  }

	WL_DEBUG_PRINT("in xbee_init\n");

#ifdef ROBOT

	//enable the receiving interrupt

#ifdef FIREFLY

	UCSR0B |= _BV(RXCIE) | _BV(RXEN);

#ifdef BAYBOARD

	UCSR1B |= _BV(RXCIE1);

#else

	UCSR1B |= _BV(RXCIE);

#endif

#endif

	sei();

#else

	xbee_stream = open(xbee_com_port, O_RDWR);

	if (xbee_stream == -1/* || lockf(xbee_stream, F_TEST, 0) != 0*/)

	{

		WL_DEBUG_PRINT("Failed to open connection to XBee on port ");

		WL_DEBUG_PRINT_INT(xbee_com_port);

		WL_DEBUG_PRINT(".\n");

		return -1;

	} else {

	  WL_DEBUG_PRINT("Successfully opened connection to XBee on port ");

		WL_DEBUG_PRINT_INT(xbee_com_port);

		WL_DEBUG_PRINT(".\n");

	}

	// set baud rate, etc. correctly

	struct termios options;

	tcgetattr(xbee_stream, &options);

	cfsetispeed(&options, B9600);

	cfsetospeed(&options, B9600);

	options.c_iflag &= ~ICRNL;

	options.c_oflag &= ~OCRNL;

	options.c_cflag |= (CLOCAL | CREAD);

	options.c_cflag &= ~PARENB;

	options.c_cflag &= ~CSTOPB;

	options.c_cflag &= ~CSIZE;

	options.c_cflag |= CS8;

	options.c_lflag &= ~ICANON;

	options.c_cc[VMIN] = 1;

	options.c_cc[VTIME] = 50;

	if (tcsetattr(xbee_stream, TCSANOW, &options))

	{

		WL_DEBUG_PRINT("Error setting attributes.\n");

		return -1;

	}

	xbee_listen_thread = (pthread_t*)malloc(sizeof(pthread_t));

	if (xbee_listen_thread == NULL)

	{

		WL_DEBUG_PRINT("Malloc failed.\n");

		return -1;

	}

	int ret = pthread_create(xbee_listen_thread, NULL, listen_to_xbee, NULL);

	if (ret)

	{

		WL_DEBUG_PRINT("Failed to create listener thread.\n");

		return -1;

	}

#endif

	WL_DEBUG_PRINT("Entering command mode.\r\n");

	if (xbee_enter_command_mode() != 0) {

    usb_puts("error entering command mode\r\n");

		return -1;

	}

	WL_DEBUG_PRINT("Entered command mode.\r\n");

  // reset baud rate

  WL_DEBUG_PRINT("Resetting Baud to ");

  WL_DEBUG_PRINT(XBEE_BAUD_STR);

  WL_DEBUG_PRINT("\r\n");

  // set baud on xbee

//#if (XBEE_BAUD == 250000)

#if (XBEE_BAUD == 115200)

  xbee_send_string("ATBD7\r\n");

#elif (XBEE_BAUD == 57600)

  xbee_send_string("ATBD6\r\n");

#elif (XBEE_BAUD == 38400)

  xbee_send_string("ATBD5\r\n");

#elif (XBEE_BAUD == 19200)

  xbee_send_string("ATBD4\r\n");

#elif (XBEE_BAUD == 9600)

  // already at this baud rate

  //xbee_send_string("ATBD3\r\n");

#else

  WL_DEBUG_PRINT("undefined baud rate\r\n");

#endif  

  // exit command mode

  xbee_wait_for_ok();

  WL_DEBUG_PRINT("got ok from baud reset\r\n");

  xbee_send_string("ATCN\r\n");

  xbee_wait_for_ok();

  WL_DEBUG_PRINT("got ok from exiting command mode\r\n");

  // set UART baud

#if (XBEE_BAUD == 250000)

  UBRR1H = 0x00;

  UBRR1L = 3;

  UCSR1A |= _BV(U2X1);

#elif (XBEE_BAUD == 115200)

  UBRR1H = 0x00;

  UBRR1L = 8;

  UCSR1A |= _BV(U2X1);

#elif (XBEE_BAUD == 57600)

  UBRR1H = 0x00;

  UBRR1L = 16;

  UCSR1A |= _BV(U2X1);

#elif (XBEE_BAUD == 38400)

  UBRR1H = 0x00;

  UBRR1L = 25;

  UCSR1A |= _BV(U2X1);

#elif (XBEE_BAUD == 19200)

  UBRR1H = 0x00;

  UBRR1L = 51;

  UCSR1A |= _BV(U2X1);

#elif (XBEE_BAUD == 9600)

  /* this is the default baud rate, so do nothing

  UBRR1H = 0x00;

  UBRR1L = 103;

  UCSR1A |= _BV(U2X1);*/

#else //Baud rate is defined in the header file, we should not get here

  return 0;

#endif

  delay_ms(50);

  // enter command mode

  WL_DEBUG_PRINT("entering command mode 2\r\n");

  xbee_send_string("+++");

  xbee_wait_for_ok();

  WL_DEBUG_PRINT("entered command mode 2\r\n");

  if (xbee_enter_api_mode() != 0) {

    WL_DEBUG_PRINT("can't enter api mode\r\n");

		return -1;

	}

	WL_DEBUG_PRINT("Entered api mode.\r\n");

	if (xbee_exit_command_mode() != 0) {

    WL_DEBUG_PRINT("can't exit command mode\r\n");

	  return -1;

	}

	WL_DEBUG_PRINT("Left command mode.\r\n");

  // get baud rate

	if (xbee_send_read_at_command("BD")) {

    WL_DEBUG_PRINT("can't send BD command\r\n");

		return -1;

	}

	WL_DEBUG_PRINT("Getting ATBD rate.\n");

  // get MY address

	if (xbee_send_read_at_command("MY")) {

    WL_DEBUG_PRINT("can't send my command\r\n");

		return -1;

	}

	WL_DEBUG_PRINT("Getting ATMY address.\n");

#ifndef ROBOT

	int i;

	for (i = 0; xbee_address == 0 && i < XBEE_GET_PACKET_TIMEOUT; i++) {

	  ret = xbee_get_packet(NULL);

	  usleep(1000);

/* 	  if (ret == -1) { */

/* 	    WL_DEBUG_PRINT("xbee_get_packet(NULL) failed.\n"); */

/* 	    return -1; */

/* 	  } */

	}

#else

	//wait to return until the address is set

  //TODO: this shouldn't wait indefinitely.  There should be some sort of reasonable timeout

  // so if the address is never set right, an error can be returned instead of having the

  // robot hang forever

	while (xbee_address == 0) {

	  xbee_get_packet(NULL);

	}

#endif

	WL_DEBUG_PRINT("Got ATMY address.\n");

#ifndef ROBOT

	if (i == XBEE_GET_PACKET_TIMEOUT) { // We timed-out.

	  WL_DEBUG_PRINT("xbee_get_packet timed out.\n");

	  return -1;

	} else {

	  return 0;

	}

#else

	return 0;

#endif

}

void xbee_terminate()

{

	#ifndef ROBOT

	pthread_cancel(*xbee_listen_thread);

    pthread_join(*xbee_listen_thread, NULL);

	free(xbee_listen_thread);

	lockf(xbee_stream, F_ULOCK, 0);

	close(xbee_stream);

  #else

  xbee_exit_api_mode();

	#endif

}

static int xbee_send(char* buf, int size)

#ifndef ROBOT