Colony

/** @file colonet.c

 * @brief Colonet library for DRAGONFLY colony robots

 *

 * @author Eugene Marinelli

 * @date 4/17/07

 * 

 * @bug Handler registration not tested

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

 */

#include <dragonfly_lib.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];

/* Internal function prototypes */

static void packet_string_to_struct(ColonetPacket* dest_pkt, char* pkt_buf);

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

/* Public functions */

int colonet_handle_message(unsigned char robot_id, char* pkt_buf)

{

  ColonetPacket pkt;

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

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

  packet_string_to_struct(&pkt, pkt_buf);

  if(pkt.msg_dest != GLOBAL_DEST && pkt.msg_dest != robot_id){ 

    //message not intended for us

    return 1;

  }

  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(pkt.msg_type == COLONET_REQUEST){

    /* 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:

      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(pkt.msg_type == COLONET_COMMAND){

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

      }

      return 0;

    }

    switch(pkt.msg_code){

    default:

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

             pkt.msg_code);

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

      //LCD

    case LCD_INIT:

      lcd_init();

      break;

    case LCD_CLEAR_SCREEN:

      lcd_clear_screen();

      break;

    case LCD_PUTBYTE:

      lcd_putbyte(args[0]);

      break;

    case LCD_PUTCHAR:

      lcd_putchar((char)args[0]);

      break;

    case LCD_PUTSTR:

      lcd_putstr((char*)args);

      break;

    case LCD_GOTOXY: 

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

      break;

    case LCD_PUTINT: 

      lcd_putint(int_args[0]);

      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:

      motors_init();

      break;

    case MOTOR1_SET:

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

      break;

    case MOTOR2_SET:

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

      break;

    case MOTORS_OFF:

      motors_off();

      break;

      /*

    case MOVE:

      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:

      analog_init();

      break;

      //BOM

    case BOM_ON:

      bom_on();

      break;

    case BOM_OFF:

      bom_off();

      break;

    case OUTPUT_HIGH:

      output_high(int_args[0]);

      break;

    case OUTPUT_LOW:

      output_low(int_args[0]);

      break;

      //Dio

    case DIGITAL_OUTPUT:

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

      break;

    }

  }else{

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

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

}

/* Private functions */

static void packet_string_to_struct(ColonetPacket* dest_pkt, char* pkt_buf)

{

  int i;

  printf("\npacket:");

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

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

  }

  printf("\n");

  dest_pkt->prefix[0] = pkt_buf[0];

  dest_pkt->prefix[1] = pkt_buf[1];

  dest_pkt->token_src = pkt_buf[2];

  dest_pkt->token_dest = pkt_buf[3];

  dest_pkt->msg_dest = pkt_buf[4];

  dest_pkt->msg_type = pkt_buf[5];

  dest_pkt->msg_code = pkt_buf[6];

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

    dest_pkt->data[i] = pkt_buf[7+i];

  }

  dest_pkt->num_robots = pkt_buf[PACKET_SIZE-2];

  dest_pkt->checksum = pkt_buf[PACKET_SIZE-1];

}

/* 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;

}