Colony

/** @file colonet_wireless.c

 *

 * @brief Implementation of server-side colonet wireless library

 *

 * @author Eugene Marinelli

 */

/********************************* Includes **********************************/

#include <stdio.h>

#include <string.h>

#include <stdlib.h>

#include <unistd.h>

#include <pthread.h>

#include <signal.h>

#include <wireless.h> // Colonet wireless library.

#include <wl_token_ring.h>

#include <colonet_wireless.h>

/******************************* Definitions *********************************/

//Enable debug printouts

//#define DEBUG 1

#ifdef DEBUG

#define dbg_printf(...) printf(__VA_ARGS__)

#define dbg_fprintf(...) fprintf(__VA_ARGS__)

#else

#define dbg_printf(...)

#define dbg_fprintf(...)

#endif

static pthread_t listener_thread;

static char wl_port[40];

/************************* Internal prototypes *******************************/

static void* listen(void* args);

/**************************** Public functions *******************************/

/**

 * @brief Initializes the wireless library

 *

 * @param wl_port_ The port to listen for wireless messages on

 *

 * @return 0 on success, negative error code on failure

 */

int colonet_wl_init(char* wl_port_) {

  strncpy(wl_port, wl_port_, 40);

  wl_set_com_port(wl_port);

  fprintf(stderr, "Calling wl_init(%s)...\n", wl_port);

  if (wl_init() != 0) {

    fprintf(stderr, "wl_init failed.\n");

    return -1;

  }

  fprintf(stderr, "Setting channel\n");

  wl_set_channel(0xF);

  wl_set_pan(0x3332);

  /*wl_token_ring_register();

  fprintf(stderr, "Joining token ring...\n");

  if (wl_token_ring_join() != 0) {

    fprintf(stderr, "Failed to join token ring.\n");

    return -1;

  }

  fprintf(stderr, "Joined token ring.\n");*/

  return 0;

}

/**

 * @brief This function will kill the thread that is listening for wireless messages

 */

void colonet_wl_kill_listener_thread() {

  pthread_kill(listener_thread, 9);

}

void colonet_wl_join() {

  pthread_join(listener_thread, NULL);

}

/**

 * @brief This function spawns a thread to listen for wireless messages

 *

 * @return 0 on success, negative error code on failure

 */

int colonet_wl_run_listener_thread() {

  dbg_printf("Spawning listener thread...\n");

  if (pthread_create(&listener_thread, NULL, listen, NULL)) {

    perror("pthread_create");

    return -1;

  }

  return 0;

}

/**

 * @brief Used to get the number of robots in the token ring

 *

 * @return The result of wl_token_get_num_robots()

 */

int colonet_get_num_robots(void) {

  return wl_token_get_num_robots();

}

/**

 * @brief Gets a list of the xbee ids of the ones in the token ring

 *

 * @param numrobots A pointer to the variable where you want the number of robots to be stored

 *

 * @return A dynamically allocated piece of memory that contains the list of xbee ids.

 *   The function calling this function must free this memory when done with it.

 */

void colonet_get_xbee_ids(int ids[]) {

  wl_token_iterator_begin();

  int i = 0;

  while (wl_token_iterator_has_next()) {

    ids[i++] = wl_token_iterator_next();

  }

}

/**

 * @brief Gets the sensor matrix from the wireless library

 *

 * @param numrobots A pointer to the variable where you want the number of robots to be stored

 * @param ids_ A pointer to a pointer that you want to point to the list of xbee ids.  This memory is dynamically

 * allocated and should be freed by the calling process when it is done with it

 *

 * @return The 2d matrix reprsenting the sensor matrix.  This memory is dynamically allocated and should be freed by

 * the calling process when it is done with it

 */

void colonet_get_sensor_matrix(int* ids, SensorReading*** m) {

  int num_robots = colonet_get_num_robots();

  colonet_get_xbee_ids(ids);

  for (int i = 0; i < num_robots; i++) {

    for (int j = 0; j < num_robots; j++) {

      m[i][j] = wl_token_get_sensor_reading(ids[i], ids[j]);

    }

  }

}

/** @brief Analogous to the "SIGNAL" or "ISR" function on the robots.

 * Listens for bytes on the wireless port and constructs packets based on them.

 * Not part of the ColonetWireless class since a function pointer must be

 * passed in starting the thread that runs it (tricky or impossible if it's

 * a class function).

 *

 * @param args Pointer to arguments.  Can be safely casted to ListenerArgs type

 * @return NULL

 */

static void* listen(void* args) {

  args = args; //TODO: These are just here to get rid of compiler warnings.

  fprintf(stderr, "Called listen.\n");

  while (1) {

    wl_do();

    //this sleep is here so the thread this runs in doesn't hog the cpu

    pthread_yield();

  }

  fprintf(stderr, "Listen is returning.\n");

  wl_terminate();

  return NULL;

}