Colony

#include "colonet_dragonfly.h"

static void colonet_handle_receive(char type, int source, unsigned char* packet, int length) {

    	wl_send_robot_to_robot_global_packet(colonet_pgh.groupCode, BATTERY,

		"brouhaha", 8, 0xA, 0);

    // Battery

    case BATTERY:

      usb_puts("Got battery request.\n");

/*

      int xbeeDongleID = 0xA;  // too bad this is hard-coded.

      char data[20];

      data[0] = 0;  // client ID specified in bytes 0-3

      data[1] = 0;

      data[2] = 0;

      data[3] = 0;

      data[4] = 0;  // ???

      data[5] = RESPONSE_TO_CLIENT_REQUEST;

      data[6] = ' ';

      data[7] = BATTERY;

      data[8] = ' ';

      data[9] = (char) wl_get_xbee_id();  // robot number (self)

      data[10] = ' ';

      data[11] = (char) battery8();  // battery reading

      data[12] = '\0';

      wl_send_robot_to_robot_global_packet(colonet_pgh.groupCode, COLONET_RESPONSE, data, 13, xbeeDongleID, 0);

      usb_puts("lol \n");

*/

      break;

      //Orb

   	  sprintf(buf, "calling motor1_set [%i] [%i]\n", args[0], args[1]);

      //Dio

INCLUDES = -I../ -I../../../libwireless/lib -I../../../../lib/include/libdragonfly

// Message types

typedef enum {COLONET_COMMAND = 13, COLONET_REQUEST = 14,

              COLONET_RESPONSE = 15} ColonetMessageType;

#define PACKET_DATA_LEN 7

//#define MAX_NUM_ROBOTS 7

  short client_id; // ID number of the client sending the packet.

                   // Size of short is common to robots and server.

//Colonet Client commands

//TODO: renamed these to end in LENGTH

#define MAX_COMMAND_LEN 128

#define MAX_RESPONSE_LEN 1024

#define SEND_TO_ROBOT      0x00

#define REQUEST_FROM_SERVER 0x01

#define RESPONSE_TO_CLIENT_REQUEST 0x02

/** @file wirelessMessageHandler.cpp

 *

 *  @author Jason Knichel

 *  @author Eugene Marinelli

 *

 * @bug Weird segfault when processing tokens

 */

#include <string.h>

#include <stdio.h>

#include <unistd.h>

#include <ctype.h>

#include <errno.h>

#include <colonet_wireless.h>

#include <wirelessMessageHandler.h>

#include <options.h>

#include <ColonetServer.h>

/* Globals */

extern ColonetServer colonet_server;

/* Public functions */

int wirelessMessageHandler(unsigned char type, short source, int dest, unsigned char* data, int len) {

  printf("Server received wireless message!!!\n");

  printf("server received this packet: ");

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

    printf("%d ", data[i]);

  }

  printf(" type: %d, source: %d, dest: %d\n", type, source, dest);

  return colonet_server.process_received_wireless_message(type, source, dest, data, len);

}

Command::~Command() {

}

  if (!connection_pool) {

  if (!command) {

    return -1;

  }

  if (pool_index < 0) {

    return -1;

  }

int Command::parse_send_to_robot(int number_tokens, char tokens[MAX_TOKENS][MAX_TOKEN_SIZE], int pool_index) {

  int i;

  unsigned char int_tokens[MAX_TOKENS];

  int number_int_tokens = number_tokens;

  unsigned char arguments[PACKET_DATA_LEN];

  for (i = ROBOT_COMMAND_OFFSET; i < number_int_tokens; i++) {

  for (i = ROBOT_COMMAND_LEN; i < number_int_tokens-ROBOT_COMMAND_OFFSET; i++) {

  fprintf(stderr, "Calling colonet_wl_send(%d, %d, %d, arguments)\n",

          int_tokens[0], int_tokens[1], int_tokens[2]);

  if (colonet_wl_send((short)pool_index, int_tokens[0], (ColonetMessageType)int_tokens[1], int_tokens[2], arguments)

      != 0) {

static MsgHandlerFunction message_handler;

int colonet_wl_init(char* wl_port_, MsgHandlerFunction message_handler_, char* log_filename_) {

  message_handler = message_handler_;

int colonet_wl_send(short client_source, short dest, ColonetMessageType msg_type, unsigned char msg_code,

                     unsigned char* args) {

    printf("sending to specific robot.\n");

  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);

    log_packet(packet, len);

  message_handler(type, source, pkt.client_id, packet, len);

/**

 *

 *  @author Jason Knichel

 *

 */

#ifndef WIRELESSMESSAGEHANDLER_H

#define WIRELESSMESSAGEHANDLER_H

#include <colonet_wireless.h>

int wirelessMessageHandler(unsigned char type, short source, int dest,

                           unsigned char* data, int len);

#endif

  int start_listening();

  int run_server();

  int process_received_wireless_message(unsigned char type, short source, int dest,

					unsigned char * data, int len);

  int initialize_wireless();

 private:

typedef int(*MsgHandlerFunction)(unsigned char type, short source, int dest, unsigned char* data, int len);

int colonet_wl_init(char* wl_port, MsgHandlerFunction message_handler_, char* log_filename_);

int colonet_wl_send(short client_source, short dest, ColonetMessageType msg_type, unsigned char msg_code,

    printf("Read buffer is %s\n", read_buffer[pool_index]);

	read_buffer_size[pool_index] = 0;

	break;

	break;

	break;

	temporary_command_buffer[command_length-2] = '\0';

	printf("The command was too long.  Tossing command out.\n");

	break;

	printf("There was an error parsing command\n");

	break;

#include <wirelessMessageHandler.h>

	if (errno == EMFILE) {

	  printf("\tWhen attempting to accept a connection, "

		 "reached the per process limit of file descriptors."

		 "  Dropping the new connection.\n");

	  continue;

	} else {

	  printf("\tThere was an error when attempting to accept a connection");

	}

	continue;

	       inet_ntoa(client_addr.sin_addr));

	printf("\tThere was an error when trying to add a client to the connection pool.");

	continue;

	       inet_ntoa(client_addr.sin_addr));

int ColonetServer::process_received_wireless_message(unsigned char type, short source, int dest,

                                                     unsigned char * data, int len) {

  if (type == COLONET_RESPONSE) {

    printf("response\n");

    //Greg's code

    char * buffer = (char *) &(data[5]);

    int buflen = strlen(buffer);

    buffer[buflen] = '\n';

    buflen++;

    if (connection_pool.write_to_client(dest, buffer, buflen) == ERROR_INVALID_CLIENT_ID) {

      printf("The robot wanted to pass the data to a client not in the pool.\n");

      return -1;

    }

    printf("Put data in write buffer for client.\n");

  } else {

    printf("not a response\n");

  if (colonet_wl_init(optionsG.wireless_port, wirelessMessageHandler, log_filename) != 0) {

COLONETCPPFILES = Main.cpp ColonetServer.cpp wirelessMessageHandler.cpp ConnectionPool.cpp Command.cpp colonet_wireless.cpp

/*

Wireless

Eugene Marinelli

(See .h for details)

*/

/* Includes */

#include <firefly+_lib.h>

#include <string.h>

#include <avr/interrupt.h>

#include "wireless.h"

/* Constants */

#define PREFIX0 'R'

#define PREFIX1 'C'

//wl_buf positions

#define WL_SRC_LOC 2

#define WL_DEST_LOC 3

#define WL_DATA_START 4

#define WL_TIMEOUT 4

#define WL_TIMEOUT_INITIAL 4

/* Globals */

static WL_Packet latest_packet;

static WL_Packet tmp_packet;

static char new_message; //set to 1 if there is a new message, else 0

static char listener_addr; //address that the robot is listening on

static int message_length; //length of the messages sent/received

//static unsigned char wl_to_flag;

//static unsigned int wl_to_count;

//static unsigned int wl_to_max;

/* Internal function headers */

static void wl_timeout_handler(void);

/* External function definitions */

int wl_init(int msg_len, char listener_address){

  if(msg_len > WL_MSG_MAX_LEN){

    printf("Error: %s: message length must be less than %d\n", __FUNCTION__, 

	   WL_MSG_MAX_LEN);

    return -1;

  }

  message_length = msg_len;

  listener_addr = listener_address;

  rtc_init(PRESCALE_DIV_128, 64, &wl_timeout_handler); 

  /* executes the function about every 1/4 sec */

  //wl_to_flag = 0;

  //wl_to_count = 0; 

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

  sei();

  new_message = 0;

  memset(&latest_packet, 0, sizeof(WL_Packet));

  return 0;

}

int wl_send(char* msg, char dest){

  int i = 0;

  WL_Packet packet;

  if(strlen(msg) > WL_MSG_MAX_LEN){

    printf("Error: %s: message is too long\n", __FUNCTION__);

    return -1;

  }

  wl_create_packet(msg, listener_addr, dest, &packet);

  serial_putchar(packet.prefix[0]);

  serial_putchar(packet.prefix[1]);

  serial_putchar(packet.src);

  serial_putchar(packet.dest);

  for(i = 0; i < message_length; i++){

    serial_putchar(packet.msg[i]);

  }

  serial_putchar(packet.checksum);

  return 0;

}

int wl_recv(char* msgbuf, char* src, char* dest){

  if(!new_message){

    return 0; //no new message

  }

  strcpy(msgbuf, latest_packet.msg);

  *src = latest_packet.src;

  *dest = latest_packet.dest;

  new_message = 0;

  return strlen(msgbuf);

}

char wl_get_checksum(WL_Packet* packet){

  char checksum = 0;

  int i;

  checksum += packet->src;

  checksum += packet->dest;

  for(i = 0; i < message_length; i++) {

    checksum += packet->msg[i];

  }

  return checksum;

}

int wl_create_packet(char* msg, char src, char dest, WL_Packet* packet){

  packet->prefix[0] = 'R';

  packet->prefix[1] = 'C';

  packet->src = src;

  packet->dest = dest;

  memset(packet->msg, 0, message_length);

  strcpy(packet->msg, msg);

  packet->checksum = wl_get_checksum(packet);

  return 0;

}

/* Internal function definitions */

//use on new avr-libc

//ISR(USART0_RX_vect)

SIGNAL(SIG_USART0_RECV){

  char wl_input = UDR0;

  static unsigned int wl_index = 0;

  static unsigned char wl_chksum = 0;

  if(wl_index == 0){

    //first packet? - check chksum to make sure we're not beginning

    if(wl_input == PREFIX0) {

      wl_to_count = 0; //at this point we're not timing out

      wl_index = 1;

    }

  }else if(wl_index == 1){

    if(wl_input != PREFIX1){

      wl_index = 0;

    }else{

      wl_index = WL_SRC_LOC;

    }

  }else if(wl_index == WL_SRC_LOC){

    tmp_packet.src = wl_input;

    wl_index = WL_DEST_LOC;

  }else if(wl_index == WL_DEST_LOC){

    tmp_packet.dest = wl_input;

    wl_index = WL_DATA_START;

  }else if(wl_index < WL_DATA_START + message_length){

    //getting the rest of the packet

    tmp_packet.msg[wl_index-WL_DATA_START] = wl_input;

    wl_index++;

  }else if(wl_index == WL_DATA_START + message_length){

    //we are at checksum location

    tmp_packet.checksum = wl_input;

    wl_chksum = wl_get_checksum(&tmp_packet);

    if(tmp_packet.checksum == wl_chksum) {

      //checksum is correct

      memcpy(&latest_packet, &tmp_packet, sizeof(WL_Packet));

      new_message = 1;

    } else { //chksum failed

      serial1_putchar('F');

      lcd_putchar('F');

      printf("checksum failed - was %d, should have been %d\n", wl_input, 

	     wl_chksum);

    }

    wl_index = 0;

  }

}

/*

  \fn wl_timeout_handler

  Handles the timeout case.

  A time out occurs about every 1/4 second.  This function is called each time 

  one occurs. In the case where a robot first turns on, it will wait for a long

  time before declaring itself the leader.  Otherwise, if enough time-outs 

  occur in a row, wl_to_flag will be set to 1.  This will cause a timeout 

  packet to be sent next time around

*/

static void wl_timeout_handler( void ) {

  //increment the total number of wireless time-outs

  wl_to_count++;

  static int wl_first_to_count =  0;

  static int first_time = 1;

  //first time time out routine - listen for a long time

  if(first_time) {

    if(wl_to_count > WL_TIMEOUT_INITIAL) {

      //increment the counter for first timeouts

      wl_first_to_count++;

      //you don't actually want to send out a packet 

      //-- you would confuse other robots

      wl_to_flag = 0;

      wl_to_count = 0;

    }

    first_time = 0;

  } else {

    //not the first time - regular time out routine

    //if you have enough timeouts

    if(wl_to_count > WL_TIMEOUT) {

      //lcd_putchar('z');

      //change the time-out flag to 1

      wl_to_flag = 1;

      wl_to_count = 0;

    }

  }

}

#ifndef WIRELESS_H

#define WIRELESS_H

/*

Wireless - wireless functions for Colony

Eugene Marinelli

7/22/06

*/

#define WL_MSG_MAX_LEN 16

#define WL_PACKET_MAX_LEN (WL_MSG_MAX_LEN+5)

#define GLOBAL_DEST 200

typedef struct {

  char prefix[2];

  char src;

  char dest;

  char msg[WL_MSG_MAX_LEN];

  char checksum;

} WL_Packet;

/* wl_init

   msg_len (< WL_MSG_MAX_LEN) is the length of the messages sent 

   in the packets.

   listener_address is the address to listen on

*/

int wl_init(int msg_len, char listener_address);

int wl_send(char* msg, char dest);

int wl_recv(char* msgbuf, char* src, char* dest);

// get most recent valid message - implement message queue later

int wl_create_packet(char* msg, char src, char dest, WL_Packet* packet);

char wl_get_checksum(WL_Packet* packet);

#endif

	void (*handle_receive) (char type, int source, unsigned char* packet,

							int length);

	#ifdef ROBOT

	#ifdef FIREFLY

	#else

	#endif

	#else

	#endif

	for (i = 0; i < WL_MAX_PACKET_GROUPS; i++)

			wl_packet_groups[i]->unregister != NULL)

void wl_send_pan_packet(char group, char type,

		char* data, int len, char frame)