Colony

/* Eugene Marinelli, ...

 * 10/26/06

 * 

 * colonet.c - Colonet library for colony robots

 */

#include <firefly+_lib.h>

#include "colonet.h"

typedef struct {

  unsigned char msgId;

  void (*handler)(void);

} UserHandler;

/* Globals (internal) */

static UserHandler user_handlers[USER_DEFINED_MSG_TOTAL];

/* Internal function prototypes */

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

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;

  /* Hack - could be done more 1337ly with casting, but this way we don't have

   * to worry about little/big endian stuff */

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

    switch(pkt.msg_code){

      //Sharp

    case READ_DISTANCE:

      break;

    case LINEARIZE_DISTANCE:

      break;

    case LOG_DISTANCE:

      break;

      //Serial

    case SERIAL_GETCHAR:

      break;

    case SERIAL_GETCHAR_NB:

      break;

    case SERIAL1_GETCHAR:

      break;

    case SERIAL1_GETCHAR_NB:

      break;

      //Analog

    case ANALOG8:

      break;

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

    }

    /* TODO - send back data! */

  }else if(pkt.msg_type == COLONET_COMMAND){

    if(pkt.msg_code >= USER_DEFINED_MSG_ID_START && 

       pkt.msg_code <= USER_DEFINED_MSG_ID_END){

      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 ORB_SET_DIO:

      orb_set_dio(int_args[0], int_args[1], int_args[2]);

      break;

    case LED_INIT:

      led_init();

      break;

    case LED_USER:

      led_user(int_args[0]);

      break;

    case ORB_SET_NUM_NS:

      //orb_set_num_ns(args[0], two_bytes_to_int(args[1],args[2]),

      //               two_bytes_to_int(args[3],args[4]), 

      //               two_bytes_to_int(args[5],args[6]));

      break;

    case ORB_SET_NUM:

      //orb_set_num_ns(args[0], two_bytes_to_int(args[1],args[2]),

      //               two_bytes_to_int(args[3],args[4]), 

      //               two_bytes_to_int(args[5],args[6]));

      break;

    case ORB_SEND:

      //orb_send();

      break;

      //Motors

    case MOTORS_INIT:

      motors_init();

      break;

    case MOTOR1_SET:

      printf("motor1_set %d %d\n", int_args[0], int_args[1]);

      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;

      */

      //OLED

    case DRAWCIRCLE:

      //drawcircle((char)args[0], (char)args[1], (char)args[2], 

      //           (int)two_bytes_to_int(args[3], args[4]),

      //           (int)two_bytes_to_int(args[5], args[6]));

      break;

      /*

      //Packet isn't large enough for args of this function

    case DRAWRECT:

      break;

      */

    case DRAWLINE:

      //drawline((char)args[0], (char)args[1], (char)args[2], (char)args[3], 

      //         (int)int_args[2]);

      break;

    case DRAWCOPY:

      //drawcopy((char)args[0], (char)args[1], (char)args[2], (char)args[3], 

      //         (char)args[4], (char)args[5]);

      break;

    case DRAWFILL:

      //drawfill((char)args[0]);

      break;

    case DRAWCLEAR:

      //drawclear();

      break;

    case DRAWSCROLL:

      //drawscroll((char)args[0], (char)args[1], (char)args[2], (char)args[3]);

      break;

    case OLED_INIT:

      //OLED_init();

      break;

      //SHARP

    case ENABLE_IR:

      //enable_IR();

      break;

    case DISABLE_IR:

      //disable_IR();

      break;

      //SERIAL

    case SERIAL_INIT:

      serial_init(int_args[0]);

      break;

    case SERIAL_PUTCHAR:

      serial_putchar((char)args[0]);

      break;

    case SERIAL1_INIT:

      //serial1_init();

      break;

    case SERIAL1_PUTCHAR:

      serial1_putchar((char)args[0]);

      break;

    case PRINTF:

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

      break;

      //Time

    case DELAY_MS:

      delay_ms(int_args[0]);

      break;

      //Analog

    case ANALOG_INIT:

      analog_init();

      break;

    case SET_ADC_MUX:

      //set_adc_mux(int_args[0]);

      break;

    case ENABLE_ANALOG:

      //enable_analog(int_args[0]);

      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;

    case BUTTON1_WAIT:

      button1_wait();

      break;

    case BUTTON1_WAIT_LED:

      button1_wait_led();

      break;

    case BUTTON2_WAIT:

      button2_wait();

      break;

    case BUTTON2_WAIT_LED:

      button2_wait_led();

      break;

      //WL

    case WL_INIT:

      wl_init();

      break;

    case PARSE_BUFFER:

      parse_buffer();

      break;

    case WL_SEND:

      wl_send();

      break;

    case WL_CREATE_PACKET:

      wl_create_packet();

      break;

    case WL_DEATH_HANDLER:

      wl_death_handler();

      break;

    case WL_ERROR_HANDLER:

      //wl_error_handler();

      break;

    case WL_TIMEOUT_HANDLER:

      wl_timeout_handler();

      break;

    case DISPLAY_DEBUG:

      display_debug();

      break;

    case BOM_TEST:

      bom_test();

      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;

  }

  user_handlers[msgId-USER_DEFINED_MSG_ID_START].msgId = msgId;

  user_handlers[msgId-USER_DEFINED_MSG_ID_START].handler = handler;

  return 0;

}

/* Private functions */

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]

 */

unsigned int two_bytes_to_int(char high, char low)

{

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

}