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 <battery.h>

#include <colonet_defs.h>

#include <colonet_dragonfly.h>

#include <dragonfly_lib.h>

#include <math.h>

#include <string.h>

#include <wireless.h>

#define TARGET_POSITION_STOP_DISTANCE_THRESHOLD (40.0)

typedef struct {

  unsigned char msgId; //is this necessary?

  void (*handler)(void);

} UserHandler;

/* Globals (internal) */

static UserHandler user_handlers[USER_DEFINED_MSG_TOTAL];

static unsigned robot_x, robot_y;

static volatile int updated_robot_pos_ready;

#define ERROR_MAG_TO_MOTOR_DIFFENTIAL_CONST (1)

/* Internal function prototypes */

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

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

static void move_to_position_routine(unsigned x, unsigned y);

static PacketGroupHandler colonet_pgh;

/* two_bytes_to_int(char a, char b)

 * Returns int of form [high][low]

 */

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

  return (high<<8) | low;

}

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

  updated_robot_pos_ready = 0;

  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;

}

void get_absolute_position(int* x, int* y) {

  *x = robot_x;

  *y = robot_y;

}

void request_abs_position() {

  //usb_puts("requesting_abs_position\n");

  ColonetRobotServerPacket pkt;

  pkt.client_id = -1;

  pkt.msg_code = ROBOT_REQUEST_POSITION_FROM_SERVER;

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

}

static float sqrt_approx(float x) {

  float x2 = x*x;

  float x3 = x*x2;

  return 0.00014*x3 - 0.0078*x2 + 0.29*x + 0.84;

}

static float dist(float x1, float y1, float x2, float y2) {

  return sqrt_approx((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1));

}

/* cubic approximation of arctan. */

/*static void arctan(float t) {

  float t3 = t*t*t;

  return -0.039 * t3 + 0.74 * t + 0.00011;

  }*/

/** Differential is the intended left motor speed - right motor speed. */

static void set_motors_with_differential(int differential) {

  int ml, mr;

  if (differential >= 0) {

    /* Going left or straight. */

    ml = 250;

    mr = ml - differential;

  } else {

    /* Turning right. */

    mr = 250;

    ml = mr + differential;

  }

  int motor1_dir = mr < 0 ? 0 : 1;

  int motor2_dir = ml < 0 ? 0 : 1;

  motor1_set(motor1_dir, mr);

  motor2_set(motor2_dir, ml);

}

static void move_to_position_routine(unsigned target_x, unsigned target_y) {

  usb_puts("move to absolute position routine!\n");

  updated_robot_pos_ready = 0;

  request_abs_position(); // While we're doing this computation, server can be reporting next position.

  int count = 0;

  while (!updated_robot_pos_ready) {

    wl_do();

    if (count++ == 5000) { // in case the server missed it...

      request_abs_position();

      count = 0;

    }

  }

//  usb_puts("got past first loop.\n");

  unsigned last_x = robot_x, last_y = robot_y;

  char buf[40];

  //sprintf(buf, "cur dist is %f\n", dist((float)robot_x, (float)robot_y, (float)target_x, (float)target_y));

  //usb_puts(buf);

  //sprintf(buf, "radius squared is %f\n", TARGET_POSITION_STOP_DISTANCE_THRESHOLD);

  //usb_puts(buf);

//  usb_puts("entering while loop.\n");

  while (dist(robot_x, robot_y, target_x, target_y) > TARGET_POSITION_STOP_DISTANCE_THRESHOLD) {

//    usb_puts("inside while loop.\n");

    updated_robot_pos_ready = 0;

    request_abs_position(); // While we're doing this computation, server can be reporting next position.

//    usb_puts("after request_abs_position.\n");

    int cur_robot_x = robot_x;

    int cur_robot_y = robot_y;

    //usb_puts("after cur_robot_x/y = robot_x/y.\n");

    int e_x = target_x - cur_robot_x;

    int e_y = target_y - cur_robot_y;

    int v_x = cur_robot_x - last_x;

    int v_y = cur_robot_y - last_y;

    sprintf(buf, "vx:%d vy:%d rx:%d ry:%d ex:%d ey:%d\n", v_x, v_y, e_x, e_y, e_x, e_y);

    usb_puts(buf);

    int e_mag = e_x*e_x + e_y*e_y;

    int motor_differential = e_mag >> 7;// / 128;

/*

    sprintf(buf, "Current position: %d %d\n", cur_robot_x, cur_robot_y);

    usb_puts(buf);

    sprintf(buf, "Target position: %d %d\n", target_x, target_y);

    usb_puts(buf);

    float r_x = (float)target_x - cur_robot_x;

    float r_y = (float)target_y - cur_robot_y;

    float r_mag = sqrt_approx(r_x * r_x + r_y * r_y);

    r_x /= r_mag;

    r_y /= r_mag;

    float v_x = (float)cur_robot_x - last_x;

    float v_y = robot_y - last_y;

    float v_mag = sqrt_approx(v_x*v_x + v_y*v_y);

    v_x /= v_mag;

    v_y /= v_mag;

    float e_x = r_x - v_x;

    float e_y = r_y - v_y;

    float e_mag = sqrt_approx(e_x * e_x + e_y * e_y);

    sprintf(buf, "Error: <%f,%f>; mag: %f\n", e_x, e_y, e_mag);

    usb_puts(buf);

    // Motor differential proportional to magnitude of directional error.

  //  int motor_differential = (int)(e_mag * ERROR_MAG_TO_MOTOR_DIFFENTIAL_CONST);

*/

    int p_x = v_y;

    int p_y = -v_x;

    int e_trans_x = p_x * e_x + p_y * e_y;

/*

    // Determine left or right by transforming error vector to robot axes

    // Perpendicular

    float p_x = v_y;

    float p_y = -v_x;

    // Inverse matrix

    float coeff = 1.0 / (p_x * v_y - v_x * p_y);

    float mat[2][2];

    mat[0][0] = v_y * coeff;

    mat[0][1] = -v_x * coeff;

    //mat[1][0] = -p_y * coeff;     //Not needed for our purposes.

    //mat[1][1] = p_x * coeff;

    float e_trans_x = mat[0][0] * e_x + mat[0][1] * e_y;

    //float e_trans_y = mat[1][0] * e_x + mat[1][1] * e_y;  //Not needed for our purposes.

*/

    if (e_trans_x < 0) {

      motor_differential = -motor_differential;

    }

    sprintf(buf, "motor_diff: %d\n", motor_differential);

    usb_puts(buf);

    last_x = cur_robot_x;

    last_y = cur_robot_y;

    set_motors_with_differential(motor_differential);

    count = 0;

    while (!updated_robot_pos_ready) { // ghetto condition variable

      wl_do();

      if (count++ == 5000) { // in case the server missed it...

        request_abs_position();

        count = 0;

      }

    }

  }

  usb_puts("reached destination!\n");

}

/* 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= (ColonetRobotServerPacket*)packet;

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

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

  char buf[40];

  length = length;

  /*

  usb_puts("Packet received.\n");

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

  /*

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

  usb_puts(buf);*/

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

    sprintf(buf, "received colonet request: pkt msgcode is %d\n", pkt->msg_code);

    usb_puts(buf);

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

      // TODO: THIS IS NOT WORKING!  SERVER DOES NOT GET PACKET!

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

      sprintf(buf, "it's a battery request\nsent battery value: %s\n", pkt->data);

      usb_puts(buf);

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

    sprintf(buf, "colonet command ... pkt->msg_code=%d\n", pkt->msg_code);

    usb_puts(buf);

/* 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 = (unsigned)int_args[0];

      robot_y = (unsigned)int_args[1];

      updated_robot_pos_ready = 1;

      sprintf(buf, "pos is: %u %u\n", robot_x, robot_y);

      usb_puts(buf);

      break;

    case MOVE_TO_ABSOLUTE_POSITION:

      usb_puts("move to abs position2!\n");

      move_to_position_routine((unsigned)int_args[0], (unsigned)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;

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

  }

}