Index: trunk/code/projects/colonet/lib/colonet_wireless/colonet_wireless.cpp
===================================================================
--- trunk/code/projects/colonet/lib/colonet_wireless/colonet_wireless.cpp	(revision 391)
+++ trunk/code/projects/colonet/lib/colonet_wireless/colonet_wireless.cpp	(nonexistent)
@@ -1,230 +0,0 @@
-/** @file colonet_wireless.c
- *
- * @brief Implementation of server-side colonet wireless library
- *
- * @author Eugene Marinelli
- */
-
-/********************************* Includes **********************************/
-#include <string>
-#include <pthread.h>
-#include <iostream>
-#include <fstream>
-#include <unistd.h>
-#include <fcntl.h>
-#include <signal.h>
-
-#include <wireless.h> // Colonet wireless library.
-#include <wl_token_ring.h>
-
-#include "colonet_defs.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 MsgHandlerFunction message_handler;
-static bool logging_enabled;
-static char log_filename[80];
-static pthread_t listener_thread;
-static char wl_port[40];
-
-/************************* Internal prototypes *******************************/
-static void* listen(void* args);
-static void timeout_handler(void);
-static void handle_response(int frame, int received);
-static void handle_receive(char type, int source, unsigned char* packet, int len);
-static void unregister(void);
-static int log_packet(unsigned char* packet, int len);
-
-/**************************** Public functions *******************************/
-void colonet_wl_init(char* wl_port_, MsgHandlerFunction message_handler_, char* log_filename_) {
-  if (log_filename_ != NULL) {
-    logging_enabled = true;
-    strcpy(log_filename, log_filename_);
-  }
-
-  strncpy(wl_port, wl_port_, 40);
-
-  message_handler = message_handler_;
-
-  printf("Calling wl_init...\n");
-  wl_init();
-  printf("wl_init succeeded.\n");
-
-  wl_token_ring_register();
-
-  printf("Joining token ring...\n");
-  wl_token_ring_join();
-  printf("Joined token ring.\n");
-}
-
-void colonet_wl_kill_listener_thread() {
-  pthread_kill(listener_thread, 9);
-}
-
-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;
-}
-
-void colonet_wl_send(short client_source, short dest, ColonetMessageType msg_type, unsigned char msg_code,
-                     unsigned char* args) {
-  printf("colonet_wl_send: client_source:%d, dest:%d, msg_code:%d\n", client_source, dest, msg_code);
-
-  ColonetRobotServerPacket pkt;
-  pkt.client_id = client_source;
-  pkt.msg_code = msg_code;
-
-  for (int i = 0; i < PACKET_DATA_LEN; i++) {
-    pkt.data[i] = args[i];
-  }
-
-  if (dest == GLOBAL_DEST) {
-    printf("sending to global dest\n");
-
-    wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), sizeof(ColonetRobotServerPacket), 0);
-  } else {
-    printf("sending to specific robot.\n");
-    wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt),
-                                         sizeof(ColonetRobotServerPacket), dest, COLONET_RESPONSE_PACKET_FRAME_ID);
-  }
-}
-
-int colonet_get_num_robots(void) {
-  return wl_token_get_num_robots();
-}
-
-int* colonet_get_xbee_ids(int* numrobots) {
-  int num_robots = wl_token_get_num_robots();
-  int* ids = (int*)malloc(num_robots * sizeof(int));
-
-  wl_token_iterator_begin();
-
-  int* p = ids;
-  while (wl_token_iterator_has_next()) {
-    *p = wl_token_iterator_next();
-    p++;
-  }
-
-  *numrobots = num_robots;
-  return ids;
-}
-
-// Returns int**; should be freed
-int** colonet_get_sensor_matrix(int* numrobots, int** ids_) {
-  int num_robots;
-  int* ids = colonet_get_xbee_ids(&num_robots);
-
-  int** m = (int**)malloc(num_robots * sizeof(int*));
-  for (int i = 0; i < num_robots; i++) {
-    m[i] = (int*)malloc(num_robots * sizeof(int*));
-  }
-
-  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]);
-    }
-  }
-
-  *numrobots = num_robots;
-  *ids_ = ids;
-  return m;
-}
-
-/**************************** Private functions ******************************/
-
-static void timeout_handler() {
-  printf("colonet wireless - timeout!\n");
-}
-
-static void handle_response(int frame, int received) {
-  printf("handle response\n");
-}
-
-static void handle_receive(char type, int source, unsigned char* packet,
-                           int len) {
-  ColonetRobotServerPacket pkt;
-  pkt.client_id = packet[0] | (packet[1]<<8) | (packet[2]<<16) | (packet[3]<<24); //little endian
-
-  memcpy(pkt.data, packet + 5, PACKET_DATA_LEN);
-
-  if (logging_enabled) {
-    log_packet(packet, len);
-  }
-
-  message_handler(type, source, pkt.client_id, packet, len);
-
-  printf("handle receive\n");
-}
-
-static void unregister(void) {
-  printf("unregister\n");
-}
-
-/** @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) {
-  printf("Called listen.\n");
-
-  PacketGroupHandler pgh = {COLONET_PACKET_GROUP_ID,
-                            timeout_handler,
-                            handle_response,
-                            handle_receive,
-                            unregister};
-  wl_register_packet_group(&pgh);
-
-  while (1) {
-    wl_do();
-  }
-
-  wl_terminate();
-  pthread_exit(NULL);
-  return NULL;
-}
-
-/** @brief
- *
- * @param pkt Packet to be logged
- *
- * @return 0 on success, -1 on failure
- */
-int log_packet(unsigned char* packet, int len) {
-  FILE* logfile = fopen(log_filename, "a");
-
-  if (logfile == NULL) {
-    dbg_printf("%s: Error - fopen %s failed.\n", log_filename, __FUNCTION__);
-    return -1;
-  }
-
-  for (int i = 0; i < len; i++) {
-    fprintf(logfile, "%d ", *((unsigned char*)packet + i));
-  }
-  fprintf(logfile, "\n");
-
-  fclose(logfile);
-  return 0;
-}
Index: trunk/code/projects/colonet/lib/colonet_wireless/tests/colonet_wireless_test.cpp
===================================================================
--- trunk/code/projects/colonet/lib/colonet_wireless/tests/colonet_wireless_test.cpp	(revision 391)
+++ trunk/code/projects/colonet/lib/colonet_wireless/tests/colonet_wireless_test.cpp	(nonexistent)
@@ -1,85 +0,0 @@
-/* Eugene Marinelli
- * 11/2/06
- *
- * colonet_wireless_test - tests the colonet_wireless library
- */
-
-#include <iostream>
-#include <signal.h>
-
-#include "colonet_wireless.h"
-
-int msg_handler(ColonetPacket* pkt)
-{
-  printf("Handler!\n");
-
-  fprintf(stderr, 
-          "token_src:%d\n"
-          "token_dest:%d\n"
-          "msg_dest:%d\n"
-          "msg_type:%d\n"
-          "msg_code:%d\n"
-          "num_robots:%d\n",
-          pkt->token_src, pkt->token_dest, pkt->msg_dest, pkt->msg_type,
-          pkt->msg_code, pkt->num_robots);
-  
-  printf("data: ");
-
-  for(int i = 0; i < PACKET_DATA_LEN; i++){
-    printf("%d ", pkt->data[i]);
-  }
-  
-  printf("\n\n");
-
-  return 0;
-}
-
-int main()
-{
-  ColonetWireless* cw = new ColonetWireless(SERIAL_PORT, msg_handler, NULL, 
-                                            true, false);
-
-  /*
-  unsigned char orbdata[3] = {0xFF, 0xFF, 0xFF};
-  unsigned char motordata[4] = {0, 1, 0, 0xFF};
-  unsigned char motordata2[4] = {0, 0, 0, 0xFF};
-  unsigned char buzzerdata[5] = {0x01};
-  */
-
-  unsigned char data[10];
-  unsigned char msgcode;
-
-  //cw->send(GLOBAL_DEST, 0, 0, "eugene");
-
-  //cw->send(GLOBAL_DEST, COLONET_COMMAND, ORB_SET, orbdata);
-  //cw->send(GLOBAL_DEST, COLONET_COMMAND, MOTOR1_SET, motordata);
-
-  //cw->send(GLOBAL_DEST, COLONET_COMMAND, BUZZER_INIT, (unsigned char*)"");
-  //cw->send(GLOBAL_DEST, COLONET_COMMAND, BUZZER_SET_VAL, buzzerdata);
-
-  cw->run_listener_thread();
-
-  printf("spawned thread\n");
-
-  while(1){
-    printf("Code: ");
-    scanf("%x", (int*)&msgcode);
-
-    for (int i = 0; i < 5; i++) {
-      printf("Byte %d: ", i);
-      scanf("%x", (int*)&(data[i]));
-    }
-
-    cw->send(GLOBAL_DEST, COLONET_COMMAND, msgcode, data);
-
-    /*
-    cw->send(GLOBAL_DEST, COLONET_COMMAND, ORB_SET, orbdata);
-    cw->send(GLOBAL_DEST, COLONET_COMMAND, MOTOR1_SET, motordata);
-    cw->send(GLOBAL_DEST, COLONET_COMMAND, MOTOR2_SET, motordata2);
-
-    sleep(3);
-    */
-  }
-
-  return 0;
-}
Index: trunk/code/projects/colonet/lib/colonet_wireless/tests/colonet_wireless_test2.cpp
===================================================================
--- trunk/code/projects/colonet/lib/colonet_wireless/tests/colonet_wireless_test2.cpp	(revision 391)
+++ trunk/code/projects/colonet/lib/colonet_wireless/tests/colonet_wireless_test2.cpp	(nonexistent)
@@ -1,110 +0,0 @@
-/** @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;
-}
Index: trunk/code/projects/colonet/lib/colonet_wireless/colonet_wireless_legacy.cpp
===================================================================
--- trunk/code/projects/colonet/lib/colonet_wireless/colonet_wireless_legacy.cpp	(revision 391)
+++ trunk/code/projects/colonet/lib/colonet_wireless/colonet_wireless_legacy.cpp	(nonexistent)
@@ -1,626 +0,0 @@
-/** @file colonet_wireless.cpp 
- *
- * @brief Implementation of server-side colonet wireless library
- *
- * @author Eugene Marinelli
- *
- * @bug Only reads when gtkterm is open
- * @bug Fails to tokenize
- * @bug Deconstructor not implemented
- * @bug Nonsense initializations in constructor
- */
-
-/********************************* Includes **********************************/
-#include <string>
-#include <pthread.h>
-#include <iostream>
-#include <fstream>
-#include <unistd.h>
-#include <fcntl.h>
-#include "colonet_wireless.h"
-
-/******************************* Definitions *********************************/
-#define TIMEOUT_MAX 100
-#define WL_SEND_DELAY 5 //microseconds
-
-//for 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
-
-typedef struct {
-  MsgHandlerFunction handler;
-  ColonetWireless* cw;
-} ListenerArgs;
-
-/************************* Internal prototypes *******************************/
-static void* listen(void* args);
-
-/**************************** Public functions *******************************/
-ColonetWireless::ColonetWireless(char* wl_port, 
-                                 MsgHandlerFunction message_handler_, 
-                                 char* log_filename_, bool join_token_ring_,
-                                 bool ignore_token_)
-{
-  have_token = false;
-  first_packet_sent = false;
-
-  //FIX THESE INITIALIZATIONS
-  token_src = 255;
-  token_dest = 255;
-  num_robots = 0;
-  connected = false;
-  ignore_token = ignore_token_;
-
-  sprintf(wireless_port, wl_port);
-
-  if (log_filename) {
-    logging_enabled = true;
-    sprintf(log_filename, log_filename_);
-  }
-
-  join_token_ring = join_token_ring_;
-
-  message_handler = message_handler_;
-}
-
-ColonetWireless::~ColonetWireless()
-{
-  //kill listener thread
-  
-
-  //free packets in queues
-  
-}
-
-int ColonetWireless::handle_packet(ColonetPacket* pkt)
-{
-  if (logging_enabled) {
-    /* Log the packet */
-    log_packet(pkt);
-  }
-
-  /* Update what we know about the network based on the packet */
-  update_network_props(pkt);
-
-  /* Received a message */
-  if (message_handler(pkt) < 0) {
-    fprintf(stderr, "%s: message_handler failed\n", __FUNCTION__);
-    return -1;
-  }
-  
-  if ((unsigned char) pkt->token_dest == WL_ERROR_DEST) {
-    /* token_dest is an error - handle it */
-    if (handle_error(pkt) < 0) {
-      fprintf(stderr, "%s: handle_error failed\n", __FUNCTION__);
-      return -1;
-    }
-  } else if(join_token_ring && !connected) {
-    /* We want to join the token ring but we are not connected - join robot 
-     * network */
-    dbg_printf("%s: Trying to join robot network...\n", __FUNCTION__);
-
-    if (join_network(pkt) < 0) {
-      dbg_fprintf(stderr, "%s: join_network failed\n", __FUNCTION__);
-      return -1;
-    }
-  } else {
-
-    if (pkt->token_dest == token_src && join_token_ring && !ignore_token) {
-      /* Received the token - send queued message(s) */
-      dbg_printf("%s: token received\n", __FUNCTION__);
-
-      have_token = true;
-    
-      if (transmit_queued_message() < 0) {
-        fprintf(stderr, "%s: transmit_queued_message failed\n", __FUNCTION__);
-        return -1;
-      } else {
-        if (!first_packet_sent) {
-          dbg_printf("First packet sent.\n");
-          first_packet_sent = true;
-        }
-      }
-    }
-  }
-
-  return 0;
-}
-
-pthread_t* ColonetWireless::run_listener_thread()
-{
-  // pthread_t listener_thread;
-
-  pthread_t* listener_thread = (pthread_t*)malloc(sizeof(pthread_t));
-
-  dbg_printf("Spawning listener thread...\n");
-
-  ListenerArgs* args = new ListenerArgs;
-  args->cw = this;
-
-  if (pthread_create(listener_thread, NULL, listen, args) != 0) {
-    perror("pthread_create");
-    return NULL;
-  }
-
-  return listener_thread;
-}
-
-int ColonetWireless::send(unsigned char msg_dest, unsigned char msg_type, 
-                          unsigned char msg_code, unsigned char* data)
-{
-  /* Create a new packet it and queue it to be sent */
-  ColonetPacket* newpkt = create_packet(token_src, token_dest, msg_dest, 
-                                        msg_type, msg_code, data, num_robots);
-
-  if(newpkt == NULL){
-    fprintf(stderr, "Error: %s - create_packet failed\n", __FUNCTION__);
-    return -1;
-  }
-
-  if (ignore_token) {
-    send_packet(newpkt);
-  } else {
-    outbox.push(newpkt);                
-  }
-
-  return 0;
-}
-
-void ColonetWireless::clear_message_queue(void)
-{
-  while (!outbox.empty()) {
-    outbox.pop();
-  }
-}
-
-/**************************** Private functions ******************************/
-
-/** @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)
-{
-  int read_len;
-  int portfd;
-  unsigned char inbuf[128];
-  int no_packet_timeout_count = 0;
-  ColonetPacket* in_pkt;
-
-  /* Extract arguments from args */
-
-  ColonetWireless* cw = ((ListenerArgs*)args)->cw;
-
-  /* Open connection to wireless port */
-  if((portfd = open(cw->wireless_port, O_RDWR)) < 0){
-    perror("open");
-    pthread_exit(NULL);
-  }
-
-  /* loop waiting for messages */
-  while(1){
-    /* Read in some bytes (blocks until bytes are read) */
-    read_len = read(portfd, inbuf, 3);
-
-    if(read_len > 0){
-      /* Read in some bytes - this will always be true if portfd is blocking */
-      /* parse the packet or part of packet */
-      in_pkt = cw->build_packet(inbuf, read_len);
-      
-      if(in_pkt == NULL){
-        /* Packet is not ready yet - go back and read some more bytes */
-        continue;
-      }else{
-        /* Packet is complete */
-        /* close fd so that we can safely write to it in the handler */
-        close(portfd);
-
-        if (cw->handle_packet(in_pkt)) {
-          fprintf(stderr, "%s: Error - handle_packet failed\n", __FUNCTION__);
-        }
-
-        /* reopen wireless port */
-        if(!(portfd = open(cw->wireless_port, O_RDWR))){
-          perror("open");
-          pthread_exit(NULL);
-        }
-      }
-    }else if(read_len == 0){
-      // Note - this won't actually happen if read blocks
-      if(no_packet_timeout_count++ > TIMEOUT_MAX){
-        //Nothing received for a while - try to start a new token ring
-        dbg_printf("Attempting to start a token ring...\n");
-
-        cw->token_src = 0;
-        cw->token_dest = 1;
-        cw->num_robots = 0;
-  
-        ColonetPacket* tmp_pkt = cw->create_packet(cw->token_src, 
-                                                   cw->token_dest, 0, 0, 0, 
-                                                   (unsigned char*) "",
-                                                   cw->num_robots);
-        cw->send_packet(tmp_pkt);
-        free(tmp_pkt);
-      }
-    }else{ //(read_len < 0)
-      //Error
-      perror("read");
-    }
-  }
-
-  pthread_exit(NULL);
-
-  return NULL;
-}
-
-/** @brief Takes some number of bytes and tries to build a packet with them
- * State of the function is saved between callings, so parts of packets 
- * can be provided as available
- *
- * @param buf Data buffer 
- * @param len Number of bytes to build with
- */
-ColonetPacket* ColonetWireless::build_packet(unsigned char* buf, int len)
-{
-  static unsigned char packet_buf[PACKET_SIZE];
-  static int buf_pos = 0;
-
-  /* If this is pointer is returned - will either point to NULL or a complete
-   * packet */
-  ColonetPacket* complete_packet = NULL;
-
-  if (buf_pos >= PACKET_SIZE) {
-    buf_pos = 0;
-  }
-
-  for (int i = 0; i < len; i++) {
-    dbg_printf("%d ", buf[i]);
-
-    switch(buf_pos){
-    case 0: //prefix0
-      if(buf[i] == 'R'){
-        memset(packet_buf, 0, PACKET_SIZE);
-        packet_buf[0] = 'R';
-        buf_pos = 1;
-      }
-      break;
-    case 1: //prefix1
-      if(buf[i] == 'C'){
-        packet_buf[1] = 'C';
-        buf_pos = 2;
-      }else{
-        buf_pos = 0;
-      }
-      break;
-    default: 
-      /* anything except prefix0, prefix1, or checksum */
-      
-      /* Check for R,C - in this case, very likely that we missed a byte */
-      if (i > 0 && buf[i] == 'C' && buf[i-1] == 'R') {
-        packet_buf[0] = 'R';
-        packet_buf[1] = 'C';
-        
-        buf_pos = 2;
-      } else {
-        packet_buf[buf_pos] = buf[i];
-        buf_pos++;
-      }
-      break;
-    case (PACKET_SIZE-1):
-      /* At checksum location */
-      buf_pos = 0;
-      packet_buf[15] = buf[i];
-
-      /* Compute desired checksum */
-      unsigned char sum = 0;
-      for(int j = 2; j < PACKET_SIZE; j++){
-        sum += packet_buf[j];
-      }
-      
-      if(!sum){
-        //Checksum passed
-        dbg_printf("%s: checksum passed\n", __FUNCTION__);
-
-        complete_packet = create_packet(packet_buf[2], packet_buf[3], 
-                                        packet_buf[4], packet_buf[5], 
-                                        packet_buf[6], (packet_buf+7), 
-                                        packet_buf[PACKET_SIZE-2]);
-      }else{
-        dbg_printf("%s: checksum failed\n", __FUNCTION__);
-      }
-
-      break;
-    }
-  }
-
-  return complete_packet;
-}
-
-void ColonetWireless::update_network_props(ColonetPacket* pkt)
-{
-  if(first_packet_sent && pkt->token_dest == token_dest){
-    /* Another robot is sending to our dest - therefore the network
-     * doesn't know about us yet.  Try to register again. */
-    dbg_printf("Disconnected from network\n");
-    dbg_printf("%s: failed to register in network; trying again...\n", 
-               __FUNCTION__);
-    connected = false;
-
-  }
-
-  if(num_robots < pkt->num_robots){    
-    if(token_dest == 0){
-      /* If we were the last robot, we now point to newly added robot */
-      token_dest = num_robots;
-      dbg_printf("%s: token_dest updated to %d\n", __FUNCTION__, token_dest);
-    }
-
-    num_robots = pkt->num_robots;
-
-    dbg_printf("%s: num_robots updated to %d\n", __FUNCTION__, num_robots);
-  }else if(num_robots > pkt->num_robots && first_packet_sent){
-    /* A robot has left the network... hopefully not us */
-    dbg_printf("%s: number of robots decreased to %d\n", __FUNCTION__, 
-               pkt->num_robots);
-
-    num_robots = pkt->num_robots;
-  }
-}
-
-int ColonetWireless::join_network(ColonetPacket* pkt)
-{
-  dbg_printf("Attempting to join network...\n");
-
-  token_src = pkt->num_robots;
-  token_dest = 0;
-  num_robots = pkt->num_robots+1;
-
-  ColonetPacket* newpkt = create_packet(token_src, token_dest, 0, 0, 0, 
-                                        (unsigned char*)"", num_robots);
-  if(newpkt == NULL){
-    fprintf(stderr, "Error: %s - create_packet failed\n", __FUNCTION__);
-    return -1;
-  }
-
-  dbg_printf("JOIN PACKET:\n"
-             "token_src:%d\n"
-             "token_dest:%d\n"
-             "msg_dest:%d\n"
-             "msg_type:%d\n"
-             "msg_code:%d\n"
-             "num_robots:%d\n",
-             newpkt->token_src, newpkt->token_dest, newpkt->msg_dest, 
-             newpkt->msg_type, newpkt->msg_code, newpkt->num_robots);
-  
-  dbg_printf("data: ");
-
-  for(int i = 0; i < PACKET_DATA_LEN; i++){
-    dbg_printf("%d ", newpkt->data[i]);
-  }
-  dbg_printf("\n");
-
-  send_packet(newpkt);
-
-  connected = true;
-  dbg_printf("Connected to network.\n");
-
-  free(newpkt);
-
-  return 0;
-}
-
-int ColonetWireless::transmit_queued_message()
-{
-  ColonetPacket* top_pkt;
-  int ret;
-
-  if(have_token){
-    top_pkt = outbox.front();
-    if(top_pkt == NULL){
-      dbg_printf("%s: no messages to send -- Passing the token.\n", 
-                 __FUNCTION__);
-      /* If no packets need to be sent, just send a blank packet */
-      top_pkt = create_packet(token_src, token_dest, 0, 0, 0, 
-                              (unsigned char*)"", num_robots);
-    }else{
-      outbox.pop();
-    }
-
-    ret = send_packet(top_pkt);
-    free(top_pkt);
-    if(ret < 0){
-      dbg_fprintf(stderr, "%s: send_packet failed\n", __FUNCTION__);
-      return -1;
-    }
-
-    dbg_printf("%s: packet sent\n", __FUNCTION__);
-
-    have_token = false;
-  }
-
-  return 0;
-}
-
-int ColonetWireless::send_packet(ColonetPacket* pkt)
-{
-  int portfd = open(wireless_port, O_RDWR);
-
-  dbg_printf("\nsend packet: \n");
-
-  if (portfd == 0) {
-    return -1;
-  }
-
-  //write(portfd, (unsigned char*)pkt, 16);
-
-  for (int i = 0; i < PACKET_SIZE; i++) {
-    write(portfd, (unsigned char*)pkt+i, 1);
-    usleep(50); //delay here - robot wireless can't handle full data rate...
-    dbg_fprintf(stderr, "%x ", ((unsigned char*)pkt)[i]);
-  }
-  dbg_printf("\n");
-
-  close(portfd);
-
-  return 0;
-}
-
-unsigned char ColonetWireless::compute_checksum(ColonetPacket* pkt)
-{
-  char checksum = 0;
-
-  checksum -= pkt->token_src;
-  checksum -= pkt->token_dest;
-  checksum -= pkt->msg_dest;
-  checksum -= pkt->msg_type;
-  checksum -= pkt->msg_code;
-
-  for(int i = 0; i < PACKET_DATA_LEN; i++){
-    checksum -= pkt->data[i];
-  }
-
-  checksum -= pkt->num_robots;
-
-  return checksum;
-}
-
-ColonetPacket* ColonetWireless::create_packet(unsigned char token_src_, 
-                                              unsigned char token_dest_,
-                                              unsigned char msg_dest, 
-                                              unsigned char msg_type, 
-                                              unsigned char msg_code, 
-                                              unsigned char* data,
-                                              unsigned char num_robots_)
-{
-  ColonetPacket* dest_packet = new ColonetPacket;
-
-  dest_packet->prefix[0] = 'R';
-  dest_packet->prefix[1] = 'C';
-
-  dest_packet->token_src = token_src_;
-  dest_packet->token_dest = token_dest_;
-    
-  dest_packet->msg_dest = msg_dest;
-  dest_packet->msg_type = msg_type;
-  dest_packet->msg_code = msg_code;
-
-  memset(dest_packet->data, 0, PACKET_DATA_LEN);
-  memcpy(dest_packet->data, (char*)data, PACKET_DATA_LEN);
-
-  dest_packet->num_robots = num_robots_;
-  dest_packet->checksum = compute_checksum(dest_packet);
-
-  //printf("Made this packet: \n");
-  //print_packet(dest_packet);
-
-  return dest_packet;
-}
-
-void ColonetWireless::print_packet(ColonetPacket* pkt)
-{
-  int i;
-
-  printf("Prefix: %c%c\n", pkt->prefix[0], pkt->prefix[1]);
-  printf("Token src: %d\n", pkt->token_src);
-  printf("Token dest: %d\n", pkt->token_dest);
-  printf("Msg dest: %d\n", pkt->msg_dest);
-  printf("Msg type: %d\n", pkt->msg_type);
-  printf("Msg code: %d\n", pkt->msg_code);
-
-  printf("Data: ");
-  for(i = 0; i < PACKET_DATA_LEN; i++){
-    printf("%d ", pkt->data[i]);
-  }
-  printf("\n");
-  
-  printf("Num robots: %d\n", pkt->num_robots);
-  printf("Checksum: %d\n", pkt->checksum);
-}
-
-int ColonetWireless::handle_error(ColonetPacket* pkt)
-{
-  int dead_robot;
-  int errcode = ((char*)pkt)[WL_ERROR_LOC];
-
-  dbg_printf("%s: Handling error %d\n", __FUNCTION__, pkt->token_dest);
-
-  switch(errcode){ //token_dest 
-  case WL_ERRCODE_ROBOTDEATH:
-    dead_robot = ((char*)pkt)[WL_ERR_DEAD_LOC];
-
-    fprintf(stderr, "Dead robot: %d\n", dead_robot);
-    num_robots--;
-    
-    if(token_src > dead_robot){
-      if(token_src == num_robots){
-        token_src--;
-        token_dest = 0;
-      }else{
-        token_src--;
-        token_dest--;
-      }
-    }
-
-    //ensure that the last robot talks to the 0th one
-    //this won't be set if the LAST robot dies (all robots are < dead)
-    if(token_src == num_robots - 1){
-      token_dest = 0;
-    }
-
-    if(token_src == dead_robot){
-      //you took the place of the dead robot
-      //you start the token ring again
-      ColonetPacket* tmp_pkt = create_packet(token_src, token_dest, 0, 0, 0,
-                                             (unsigned char*)"", num_robots);
-      send_packet(tmp_pkt);
-      free(tmp_pkt);
-    }
-    break;
-  case WL_ERRCODE_NEEDTOCHARGE:
-    fprintf(stderr, "Need to charge error received\n");
-    break;
-  case WL_ERRCODE_CHARGESTATION:
-    fprintf(stderr, "Charge station error received\n");
-    break;
-  }
-    
-  return 0;
-}
-
-/** @brief 
- *
- * @param pkt Packet to be logged
- *
- * @return 0 on success, -1 on failure
- */
-int ColonetWireless::log_packet(ColonetPacket* pkt)
-{
-  FILE* logfile = fopen(log_filename, "a");
-
-  if (logfile == NULL) {
-    dbg_printf("%s: Error - fopen %s failed.\n", log_filename, __FUNCTION__);
-    return -1;
-  }
-
-  for (int i = 0; i < PACKET_SIZE; i++) {
-    fprintf(logfile, "%d ", *((unsigned char*)pkt+i));
-  }
-
-  fprintf(logfile, "\n");
-
-  fclose(logfile);
-
-  return 0;
-}
Index: trunk/code/projects/colonet/lib/colonet_wireless/colonet_wireless.h
===================================================================
--- trunk/code/projects/colonet/lib/colonet_wireless/colonet_wireless.h	(revision 391)
+++ trunk/code/projects/colonet/lib/colonet_wireless/colonet_wireless.h	(nonexistent)
@@ -1,51 +0,0 @@
-/** @file colonet_wireless.h
- *
- * @brief Wireless library for communicating with colony robots
- *
- * @author Eugene Marinelli
- * @date 10/10/07
- */
-
-#ifndef COLONET_WIRELESS_H_
-#define COLONET_WIRELESS_H_
-
-#include <colonet_defs.h>
-
-typedef int(*MsgHandlerFunction)(unsigned char type, short source, int dest, unsigned char* data, int len);
-
-/** @brief Initializes colonet wireless library
- *
- * @param wl_port Either SERIAL_PORT or USB_PORT (as defined in COLONET_DEFS)
- * @param log_filename Path to log file - outputs all raw data on the network
- * If set to NULL, logging is disabled.
- * @param msg_handler Function to be called when a packet is received.
- * Must take a ColonetPacket as an argument (see defn of MsgHandlerFunction)
- *
- * @return new ColonetWireless object
- */
-void colonet_wl_init(char* wl_port, MsgHandlerFunction message_handler_, char* log_filename_);
-
-/** @brief Spawns a thread which reads data from the hardware interface
- * with the colony (either a dongle or a robot programmed to relay data)
- * and runs msg_handler when a full packet is received
- *
- * @return pointer to the thread
- */
-int colonet_wl_run_listener_thread(void);
-
-void colonet_wl_kill_listener_thread(void);
-
-/**
- * @brief ...
- *
- * @return void
- */
-void colonet_wl_send(short client_source, short dest, ColonetMessageType msg_type, unsigned char msg_code,
-                     unsigned char* args);
-
-int colonet_get_num_robots(void);
-
-int** colonet_get_sensor_matrix(int* numrobots, int** ids);
-int* colonet_get_xbee_ids(int* numrobots);
-
-#endif
Index: trunk/code/projects/colonet/lib/colonet_wireless/Makefile
===================================================================
--- trunk/code/projects/colonet/lib/colonet_wireless/Makefile	(revision 391)
+++ trunk/code/projects/colonet/lib/colonet_wireless/Makefile	(nonexistent)
@@ -1,29 +0,0 @@
-# colonet_wireless_test makefile
-
-CC = gcc
-CFLAGS = -Wall -Wshadow -Wextra -g
-INCLUDES = -I../../../libwireless/lib -I../
-
-default: colonet_wireless.o
-
-tests: colonet_wireless_test colonet_wireless_test2
-
-colonet_wireless_test: colonet_wireless_test.o colonet_wireless.o Makefile
-	$(CC) $(CFLAGS) -I ../ -lpthread -o tests/colonet_wireless_test tests/colonet_wireless_test.o colonet_wireless.o
-
-colonet_wireless_test2: colonet_wireless_test2.o colonet_wireless.o Makefile
-	$(CC) $(CFLAGS) -I ../ -pthread -o tests/colonet_wireless_test2 tests/colonet_wireless_test2.o colonet_wireless.o
-
-colonet_wireless.o: colonet_wireless.cpp colonet_wireless.h
-	$(CC) $(CFLAGS) $(INCLUDES) -c colonet_wireless.cpp
-	ar src ../libcolonet_wireless.a colonet_wireless.o
-#	ranlib libcolonet_wireless.a
-
-colonet_wireless_test.o: tests/colonet_wireless_test.cpp
-	$(CC) $(CFLAGS) -I $(INCLUDES) -pthread -c tests/colonet_wireless_test.cpp
-
-colonet_wireless_test2.o: colonet_wireless_test2.cpp
-	$(CC) $(CFLAGS) -I $(INCLUDES) -pthread -c tests/colonet_wireless_test2.cpp
-
-clean:
-	rm -rf *.o tests/colonet_wireless_test tests/colonet_wireless_test2
Index: trunk/code/projects/colonet/ColonetServer/colonet_wireless.cpp
===================================================================
--- trunk/code/projects/colonet/ColonetServer/colonet_wireless.cpp	(nonexistent)
+++ trunk/code/projects/colonet/ColonetServer/colonet_wireless.cpp	(revision 392)
@@ -0,0 +1,230 @@
+/** @file colonet_wireless.c
+ *
+ * @brief Implementation of server-side colonet wireless library
+ *
+ * @author Eugene Marinelli
+ */
+
+/********************************* Includes **********************************/
+#include <string>
+#include <pthread.h>
+#include <iostream>
+#include <fstream>
+#include <unistd.h>
+#include <fcntl.h>
+#include <signal.h>
+
+#include <wireless.h> // Colonet wireless library.
+#include <wl_token_ring.h>
+
+#include "colonet_defs.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 MsgHandlerFunction message_handler;
+static bool logging_enabled;
+static char log_filename[80];
+static pthread_t listener_thread;
+static char wl_port[40];
+
+/************************* Internal prototypes *******************************/
+static void* listen(void* args);
+static void timeout_handler(void);
+static void handle_response(int frame, int received);
+static void handle_receive(char type, int source, unsigned char* packet, int len);
+static void unregister(void);
+static int log_packet(unsigned char* packet, int len);
+
+/**************************** Public functions *******************************/
+void colonet_wl_init(char* wl_port_, MsgHandlerFunction message_handler_, char* log_filename_) {
+  if (log_filename_ != NULL) {
+    logging_enabled = true;
+    strcpy(log_filename, log_filename_);
+  }
+
+  strncpy(wl_port, wl_port_, 40);
+
+  message_handler = message_handler_;
+
+  printf("Calling wl_init...\n");
+  wl_init();
+  printf("wl_init succeeded.\n");
+
+  wl_token_ring_register();
+
+  printf("Joining token ring...\n");
+  wl_token_ring_join();
+  printf("Joined token ring.\n");
+}
+
+void colonet_wl_kill_listener_thread() {
+  pthread_kill(listener_thread, 9);
+}
+
+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;
+}
+
+void colonet_wl_send(short client_source, short dest, ColonetMessageType msg_type, unsigned char msg_code,
+                     unsigned char* args) {
+  printf("colonet_wl_send: client_source:%d, dest:%d, msg_code:%d\n", client_source, dest, msg_code);
+
+  ColonetRobotServerPacket pkt;
+  pkt.client_id = client_source;
+  pkt.msg_code = msg_code;
+
+  for (int i = 0; i < PACKET_DATA_LEN; i++) {
+    pkt.data[i] = args[i];
+  }
+
+  if (dest == GLOBAL_DEST) {
+    printf("sending to global dest\n");
+
+    wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), sizeof(ColonetRobotServerPacket), 0);
+  } else {
+    printf("sending to specific robot.\n");
+    wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt),
+                                         sizeof(ColonetRobotServerPacket), dest, COLONET_RESPONSE_PACKET_FRAME_ID);
+  }
+}
+
+int colonet_get_num_robots(void) {
+  return wl_token_get_num_robots();
+}
+
+int* colonet_get_xbee_ids(int* numrobots) {
+  int num_robots = wl_token_get_num_robots();
+  int* ids = (int*)malloc(num_robots * sizeof(int));
+
+  wl_token_iterator_begin();
+
+  int* p = ids;
+  while (wl_token_iterator_has_next()) {
+    *p = wl_token_iterator_next();
+    p++;
+  }
+
+  *numrobots = num_robots;
+  return ids;
+}
+
+// Returns int**; should be freed
+int** colonet_get_sensor_matrix(int* numrobots, int** ids_) {
+  int num_robots;
+  int* ids = colonet_get_xbee_ids(&num_robots);
+
+  int** m = (int**)malloc(num_robots * sizeof(int*));
+  for (int i = 0; i < num_robots; i++) {
+    m[i] = (int*)malloc(num_robots * sizeof(int*));
+  }
+
+  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]);
+    }
+  }
+
+  *numrobots = num_robots;
+  *ids_ = ids;
+  return m;
+}
+
+/**************************** Private functions ******************************/
+
+static void timeout_handler() {
+  printf("colonet wireless - timeout!\n");
+}
+
+static void handle_response(int frame, int received) {
+  printf("handle response\n");
+}
+
+static void handle_receive(char type, int source, unsigned char* packet,
+                           int len) {
+  ColonetRobotServerPacket pkt;
+  pkt.client_id = packet[0] | (packet[1]<<8) | (packet[2]<<16) | (packet[3]<<24); //little endian
+
+  memcpy(pkt.data, packet + 5, PACKET_DATA_LEN);
+
+  if (logging_enabled) {
+    log_packet(packet, len);
+  }
+
+  message_handler(type, source, pkt.client_id, packet, len);
+
+  printf("handle receive\n");
+}
+
+static void unregister(void) {
+  printf("unregister\n");
+}
+
+/** @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) {
+  printf("Called listen.\n");
+
+  PacketGroupHandler pgh = {COLONET_PACKET_GROUP_ID,
+                            timeout_handler,
+                            handle_response,
+                            handle_receive,
+                            unregister};
+  wl_register_packet_group(&pgh);
+
+  while (1) {
+    wl_do();
+  }
+
+  wl_terminate();
+  pthread_exit(NULL);
+  return NULL;
+}
+
+/** @brief
+ *
+ * @param pkt Packet to be logged
+ *
+ * @return 0 on success, -1 on failure
+ */
+int log_packet(unsigned char* packet, int len) {
+  FILE* logfile = fopen(log_filename, "a");
+
+  if (logfile == NULL) {
+    dbg_printf("%s: Error - fopen %s failed.\n", log_filename, __FUNCTION__);
+    return -1;
+  }
+
+  for (int i = 0; i < len; i++) {
+    fprintf(logfile, "%d ", *((unsigned char*)packet + i));
+  }
+  fprintf(logfile, "\n");
+
+  fclose(logfile);
+  return 0;
+}
Index: trunk/code/projects/colonet/ColonetServer/includes/colonet_wireless.h
===================================================================
--- trunk/code/projects/colonet/ColonetServer/includes/colonet_wireless.h	(nonexistent)
+++ trunk/code/projects/colonet/ColonetServer/includes/colonet_wireless.h	(revision 392)
@@ -0,0 +1,51 @@
+/** @file colonet_wireless.h
+ *
+ * @brief Wireless library for communicating with colony robots
+ *
+ * @author Eugene Marinelli
+ * @date 10/10/07
+ */
+
+#ifndef COLONET_WIRELESS_H_
+#define COLONET_WIRELESS_H_
+
+#include <colonet_defs.h>
+
+typedef int(*MsgHandlerFunction)(unsigned char type, short source, int dest, unsigned char* data, int len);
+
+/** @brief Initializes colonet wireless library
+ *
+ * @param wl_port Either SERIAL_PORT or USB_PORT (as defined in COLONET_DEFS)
+ * @param log_filename Path to log file - outputs all raw data on the network
+ * If set to NULL, logging is disabled.
+ * @param msg_handler Function to be called when a packet is received.
+ * Must take a ColonetPacket as an argument (see defn of MsgHandlerFunction)
+ *
+ * @return new ColonetWireless object
+ */
+void colonet_wl_init(char* wl_port, MsgHandlerFunction message_handler_, char* log_filename_);
+
+/** @brief Spawns a thread which reads data from the hardware interface
+ * with the colony (either a dongle or a robot programmed to relay data)
+ * and runs msg_handler when a full packet is received
+ *
+ * @return pointer to the thread
+ */
+int colonet_wl_run_listener_thread(void);
+
+void colonet_wl_kill_listener_thread(void);
+
+/**
+ * @brief ...
+ *
+ * @return void
+ */
+void colonet_wl_send(short client_source, short dest, ColonetMessageType msg_type, unsigned char msg_code,
+                     unsigned char* args);
+
+int colonet_get_num_robots(void);
+
+int** colonet_get_sensor_matrix(int* numrobots, int** ids);
+int* colonet_get_xbee_ids(int* numrobots);
+
+#endif
Index: trunk/code/projects/colonet/ColonetServer/Makefile
===================================================================
--- trunk/code/projects/colonet/ColonetServer/Makefile	(revision 391)
+++ trunk/code/projects/colonet/ColonetServer/Makefile	(revision 392)
@@ -3,7 +3,7 @@
 CC = g++
 CFLAGS = -Wall -Wshadow -Wextra -g
 
-COLONETCPPFILES = Main.cpp ColonetServer.cpp wirelessMessageHandler.cpp ConnectionPool.cpp Command.cpp
+COLONETCPPFILES = Main.cpp ColonetServer.cpp wirelessMessageHandler.cpp ConnectionPool.cpp Command.cpp colonet_wireless.cpp
 COLONETCPPOBJECTS = $(COLONETCPPFILES:.cpp=.o)
 COLONETFILES = options.c
 COLONETOBJECTS = $(COLONETFILES:.c=.o)
@@ -29,7 +29,7 @@
 	make; ./ColonetServer
 	../vision/vision.sh
 
-ColonetServer: $(COLONETCPPFILES) $(COLONETFILES) $(LOGGINGFILES) -lcolonet_wireless
+ColonetServer: $(COLONETCPPFILES) $(COLONETFILES) $(LOGGINGFILES)
 	@echo "---begin compilation---"
 	@echo ""
 	@echo "---create object files---"
@@ -39,7 +39,7 @@
 	@echo "---finish creating object files---"
 	@echo ""
 	@echo "---link files---"
-	$(CC) $(CFLAGS) $(COLONETOBJECTS) $(COLONETCPPOBJECTS) $(LOGGINGOBJECTS) $(LIBRARY_DIRS_FOR_GCC) -lpthread -lcolonet_wireless -lwireless_colonet  $(INCLUDE_DIRS_FOR_GCC) -o $@
+	$(CC) $(CFLAGS) $(COLONETOBJECTS) $(COLONETCPPOBJECTS) $(LOGGINGOBJECTS) $(LIBRARY_DIRS_FOR_GCC) -lpthread -lwireless_colonet  $(INCLUDE_DIRS_FOR_GCC) -o $@
 	@echo "---finish linking files---"
 	@echo ""
 	@echo "---finish compilation---"
