Colony

/** @file colonet.c

 * @brief Colonet library for DRAGONFLY colony robots

 *

 * @author Eugene Marinelli

 * @date 10/10/07

 *

 * @bug Handler registration not tested

 * @bug Request reponding not implemented - only accepts commands.

 */

#include <assert.h>

#include <dragonfly_lib.h>

#include <battery.h>

#include <string.h>

#include <wireless.h>

#include <colonet_dragonfly.h>

typedef struct {

  unsigned char msgId; //is this necessary?

  void (*handler)(void);

} UserHandler;

/* Globals (internal) */

static UserHandler user_handlers[USER_DEFINED_MSG_TOTAL];

static int robot_x, robot_y;

static int server_xbee_id; // ID of server xbee; set when a colonet packet is received.

/* Internal function prototypes */

static void packet_string_to_struct(ColonetRobotServerPacket* dest_pkt, unsigned char* pkt_buf);

static unsigned int two_bytes_to_int(char high, char low);

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

static void request_abs_position(void);

static void move_to_position_routine(int x, int y);

static PacketGroupHandler colonet_pgh;

/* Public functions */

int colonet_init() {

  colonet_pgh.groupCode = COLONET_PACKET_GROUP_ID;

  colonet_pgh.timeout_handler = NULL;

  colonet_pgh.handle_response = NULL;

  colonet_pgh.handle_receive = colonet_handle_receive;

  colonet_pgh.unregister = NULL;

  // TODO this should return an error if wl_init has not been called yet.

  wl_register_packet_group(&colonet_pgh);

  robot_x = 0;

  robot_y = 0;

  server_xbee_id = -1;

  return 0;

}

/* colonet_add_message

 * Adds a user-defined message

 */

int colonet_add_message(unsigned char msgId, void (*handler)(void)) {

  if(msgId < USER_DEFINED_MSG_ID_START  /* || msgId > USER_DEFINED_MSG_ID_END */){

    return -1;

  }

  /* Register this function in the array */

  user_handlers[msgId-USER_DEFINED_MSG_ID_START].msgId = msgId;

  user_handlers[msgId-USER_DEFINED_MSG_ID_START].handler = handler;

  return 0;

}

static void request_abs_position() {

  if (server_xbee_id != -1) { // if -1, then we don't know the server's xbee id yet.

    ColonetRobotServerPacket pkt;

    pkt.client_id = -1;

    pkt.msg_code = ROBOT_REQUEST_POSITION_FROM_SERVER;

    wl_send_robot_to_robot_global_packet(colonet_pgh.groupCode, 0, (char*)&pkt, sizeof(pkt), server_xbee_id, 0);

  }

}

static void move_to_position_routine(int x, int y) {

  /* TODO emarinel - control algorithm */

  motor1_set(1, 200);

  motor2_set(1, 200);

}

/* Private functions */

/** @brief Handles colonet packets.  Should be called by parse_buffer

 * when it is determined that a colonet message has been received.

 *

 * @param robot_id The robot id

 * @param pkt_buf The packet buffer (e.g. wl_buf)

 *

 * @return -1 on error (invalid msgId), 0 on success

 */

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

  ColonetRobotServerPacket pkt;

  unsigned char* args; //up to 7 char args

  unsigned int int_args[3]; //up to 3 int (2-byte) args

  /* Globally store this id. */

  server_xbee_id = wl_source;

  usb_puts("Packet received.\n");

  char buf[40];

  sprintf(buf, "length=%d\n", length);

  int i;

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

    sprintf(buf, "%d: %d ", i, packet[i]);

    usb_puts(buf);

  }

  usb_puts("\n");

  //printf("received message from %d of length %d\n", source, length);

  ///assert(length == sizeof(ColonetRobotServerPacket));

  packet_string_to_struct(&pkt, packet);

  char buf2[40];

  sprintf(buf2, "client_id:%d, msg_code:%d\n", pkt.client_id, pkt.msg_code);

  usb_puts(buf2);

  args = pkt.data;

  int_args[0] = two_bytes_to_int(args[0], args[1]);

  int_args[1] = two_bytes_to_int(args[2], args[3]);

  int_args[2] = two_bytes_to_int(args[4], args[5]);

  if (type == COLONET_REQUEST) {

    usb_puts("type is colonet request\n");

    /* TODO - send back data! */

    switch (pkt.msg_code) {

      //Sharp

    case READ_DISTANCE:

      break;

    case LINEARIZE_DISTANCE:

      break;

    case LOG_DISTANCE:

      break;

      //Analog

    case CALL_ANALOG8:

      break;

    case CALL_ANALOG10:

      break;

    case WHEEL:

      break;

    case BATTERY:

      sprintf((char*)pkt.data, "%d", (int)battery8());

      wl_send_robot_to_robot_global_packet(colonet_pgh.groupCode, 0, (char*)&pkt, sizeof(pkt), wl_source, 0);

      break;

      //BOM

    case GETMAXBOM:

      break;

      //Dio

    case DIGITAL_INPUT:

      break;

    case BUTTON1_READ:

      break;

    case BUTTON2_READ:

      break;

      //Bumper

    case DETECT_BUMP:

      break;

    }

  } else if (type == COLONET_COMMAND) {

    usb_puts("colonet command...\n");

/* TODO uncomment this stuff */

/*     if (pkt.msg_code >= USER_DEFINED_MSG_ID_START && */

/*         pkt.msg_code <= USER_DEFINED_MSG_ID_END) { */

/*       if (user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler) { */

/*         /\* Call the user's handler function if it the function's address */

/*          * is non-zero (has been set) *\/ */

/*         user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler(); */

/*       } */

/*     } */

    switch (pkt.msg_code) {

/*     default: */

/*       printf("%s: Error - message code %d not implemented\n", __FUNCTION__, */

/*              pkt.msg_code); */

/*       break; */

    case SERVER_REPORT_POSITION_TO_ROBOT:

      robot_x = int_args[0];

      robot_y = int_args[1];

      sprintf(buf, "my position is: %d %d\n", robot_x, robot_y);

      usb_puts(buf);

      break;

    case MOVE_TO_ABSOLUTE_POSITION:

      move_to_position_routine(int_args[0], int_args[1]);

      break;

      //Buzzer

    case BUZZER_INIT:

      buzzer_init();

      break;

    case BUZZER_SET_VAL:

      buzzer_set_val(args[0]);

      break;

    case BUZZER_SET_FREQ:

      buzzer_set_freq(args[0]);

      break;

    case BUZZER_CHIRP:

      buzzer_chirp(int_args[0], int_args[1]);

      break;

    case BUZZER_OFF:

      buzzer_off();

      break;

    case ORB_INIT:

      orb_init();

      break;

    //Orb

    case ORB_SET:

      orb_set(args[0], args[1], args[2]);

      break;

    case ORB_SET_COLOR:

      orb_set_color(int_args[0]);

      break;

    case ORB_DISABLE:

      orb_disable();

      break;

    case ORB_ENABLE:

      orb_enable();

      break;

    case LED_INIT:

      break;

    case LED_USER:

      break;

    case ORB_SEND:

      //orb_send();

      break;

      //Motors

    case MOTORS_INIT:

      usb_puts("calling motors_init\n");

      motors_init();

      break;

    case MOTOR1_SET:

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

      usb_puts(buf);

      motor1_set(args[0], args[1]);

      break;

    case MOTOR2_SET:

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

      usb_puts(buf);

      motor2_set(args[0], args[1]);

      break;

    case MOTORS_OFF:

      usb_puts("calling motors_off\n");

      motors_off();

      break;

    case MOVE:

      sprintf(buf, "calling move with: %d, %d\n", args[0], args[1]);

      usb_puts(buf);

      move(args[0], args[1]);

      break;

    case XBEE_INIT:

      xbee_init();

      break;

    case XBEE_PUTC:

      xbee_putc((char)args[0]);

      break;

    case USB_INIT:

      usb_init();

      break;

    case USB_PUTC:

      usb_putc((char)args[0]);

      break;

    case PRINTF:

      printf("%s", pkt.data);

      break;

    case KILL_ROBOT:

      motors_off();

      buzzer_off();

      exit(0); //kill the robot

      break;

      //Time

    case DELAY_MS:

      delay_ms(int_args[0]);

      break;

      //Analog

    case ANALOG_INIT:

      //TODO: how do we really init the analog? this doesn't work:

      //analog_init();

      break;

      //BOM

    case BOM_ON:

      bom_on();

      break;

    case BOM_OFF:

      bom_off();

      break;

    //Dio

    case DIGITAL_OUTPUT:

      digital_output(int_args[0], int_args[1]);

      break;

    }

  } else {

    sprintf(buf, "%s: Error: Invalid colonet message type", __FUNCTION__);

    usb_puts(buf);

  }

}

static void packet_string_to_struct(ColonetRobotServerPacket* dest_pkt, unsigned char* pkt_buf) {

  memcpy(dest_pkt, pkt_buf, sizeof(ColonetRobotServerPacket));

}

/* two_bytes_to_int(char a, char b)

 * Returns int of form [high][low]

 */

static unsigned int two_bytes_to_int(char high, char low) {

  return (((unsigned int)high)<<8) + (unsigned int)low;

}