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

typedef struct {

  unsigned char msgId; //is this necessary?

  void (*handler)(void);

} UserHandler;

typedef struct {

  int up_x;

  int up_y;

  int low_x;

  int low_y;

} VirtualWall;

/* Globals (internal) */

static UserHandler user_handlers[USER_DEFINED_MSG_TOTAL];

static volatile unsigned robot_x, robot_y;

static volatile unsigned last_x, last_y;

static volatile int updated_robot_pos_ready;

static volatile int new_movement_command_received;

static volatile int robot_lost; //set to 1 if robot's position is not known by the server, else 0.

static volatile VirtualWall virtual_wall;

static int virtual_wall_available;

#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 int dist_squared(int x1, int y1, int x2, int y2);

static void move_routine(void);

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 = last_x = 0;

  robot_y = last_y = 0;

  updated_robot_pos_ready = 0;

  new_movement_command_received = 0;

  robot_lost = 1;

  virtual_wall_available = 0;

  virtual_wall.up_x = 0;

  virtual_wall.up_y = 0;

  virtual_wall.low_x = 0;

  virtual_wall.low_y = 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(ColonetRobotServerPacket), 0);

}

static int dist_squared(int x1, int y1, int x2, int y2) {

  int dx = x1 - x2;

  int dy = y1 - y2;

  return dx*dx + dy*dy;

}

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

    mr = ml - differential;

  } else {

    /* Turning right. */

    mr = 200;

    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 int get_motor_differential(int e_x, int e_y, int v_x, int v_y) {

  //  char buf[80];

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

  //  usb_puts(buf);

  return 250;

  /*

  long e_mag = sqrt_100_approx(e_x*e_x + e_y*e_y);

  long v_mag = sqrt_100_approx(v_x*v_x + v_y*v_y);

  long e_norm_x = (1000 * e_x) / e_mag;

  long e_norm_y = (1000 * e_y) / e_mag;

  long v_norm_x = (1000 * v_x) / v_mag;

  long v_norm_y = (1000 * v_y) / v_mag;

  long e_dot_v = e_norm_x*v_norm_x + e_norm_y*v_norm_y;

  */

}

static void move_routine() {

  do {    

    int count = 0;

    while (!updated_robot_pos_ready) {

      wl_do();

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

        request_abs_position();

        count = 0;

      }

    }

    wl_do();

    if (robot_lost) {

      motors_off();

    }

    updated_robot_pos_ready = 0;

    request_abs_position();

  } while (!new_movement_command_received);

}

static void move_to_position_routine(unsigned target_x, unsigned target_y) {

  new_movement_command_received = 0;

  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;

    }

  }

  int dist = 0;

  while (!new_movement_command_received &&

         (dist = dist_squared(robot_x, robot_y, target_x, target_y)) > TARGET_POSITION_STOP_DISTANCE_THRESHOLD) {

    char buf[80];

    //sprintf(buf, "dist:%d\n", dist);

    //usb_puts(buf);

    updated_robot_pos_ready = 0;

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

    if (robot_lost) {

      usb_puts("lost known\n");

      motors_off();

    } else {

      // e is the error vector (where we want to go)

      //char buf[80];

      //                sprintf(buf, "target_x:%d target_y:%d robot_x:%d robot_y:%d\n", target_x, target_y, robot_x, robot_y);

      //                usb_puts(buf);

      int e_x = (int)target_x - (int)robot_x;

      int e_y = (int)target_y - (int)robot_y;

      //                sprintf(buf, "e: (x:%d,y:%d)\n", e_x, e_y);

      //                usb_puts(buf);

      // v is the velocity vector (where we just went)

      int v_x = (int)robot_x - (int)last_x;

      int v_y = (int)robot_y - (int)last_y;

      //sprintf(buf, "v: (x:%d,y:%d)\n", v_x, v_y);

      //usb_puts(buf);

      if (abs(v_x) < 2 && abs(v_x) < 2) {

        // if we didn't move since the last check, just go forward.

        motor1_set(1, 180);

        motor2_set(1, 180);

      } else {

        int e_dot_v = e_x * v_x + e_y * v_y;

        // move according to algortihm

        int e_mag = e_x*e_x + e_y*e_y;

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

        int motor_differential = get_motor_differential(e_x, e_y, v_x, v_y);

        // p is perpendicular to v, directed to the "left" of the robot.

        int p_x = v_y;

        int p_y = -v_x;

        int dot_product_pe = p_x * e_x + p_y * e_y;

        // if the dot product of p and e is negative, we should turn right. otherwise turn left.

        if (dot_product_pe < 0) {

          motor_differential = -motor_differential;

        }

        set_motors_with_differential(motor_differential);

      }

    }

    if (robot_lost) {

      motors_off();

    }

    // once we move, ask for the position again.

    count = 0;

    while (!updated_robot_pos_ready) {

      wl_do();

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

        request_abs_position();

        count = 0;

      }

    }

  }

  motors_off();

  //  Once this function terminates, the robot be at its destination.

}

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

  length = length;

  wl_source = wl_source;

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

  /*

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

  */

  //sprintf(buf, "received message from client_id=%d of length %d\n", pkt->client_id, length);

  //usb_puts(buf);

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

      //Analog

    case WHEEL:

      break;

    case BATTERY:

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

      // NOTE: wl_send_robot_to_robot_global_packet does not work here!

      wl_send_global_packet(COLONET_PACKET_GROUP_ID, 0, (char*)pkt, sizeof(ColonetRobotServerPacket), 0);

      break;

      //BOM

    case GETMAXBOM:

      break;

    case DIGITAL_INPUT:

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

      last_x = robot_x;

      last_y = robot_y;

      usb_puts("lost\n");

      robot_lost = 1;

      motors_off();

      break;

    case SERVER_REPORT_VIRTUAL_WALL_UPPER:

      virtual_wall.up_x = int_args[0];

      virtual_wall.up_y = int_args[1];

      break;

    case SERVER_REPORT_VIRTUAL_WALL_LOWER:

      virtual_wall.low_x = int_args[0];

      virtual_wall.low_y = int_args[1];

      break;

    case SERVER_REPORT_POSITION_TO_ROBOT:

      robot_lost = 0;

      last_x = robot_x;

      last_y = robot_y;

      robot_x = (unsigned)int_args[0];

      robot_y = (unsigned)int_args[1];

      updated_robot_pos_ready = 1;

      if (robot_x < virtual_wall.up_x || robot_x > virtual_wall.low_x || robot_y < virtual_wall.up_x

          || robot_y > virtual_wall.up_y) {

        robot_lost = 1;

        motors_off();

      }

      //    sprintf(buf, "pos is: (x:%d, y:%d)\n", robot_x, robot_y);

      //usb_puts(buf);

      break;

    case MOVE_TO_ABSOLUTE_POSITION:

      new_movement_command_received = 1;

                        sprintf(buf, "move to abs (x:%u, y:%u)\n", (unsigned)int_args[0], (unsigned)int_args[1]);

                        usb_puts(buf);

      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;

      //Orb

    case ORB_INIT:

      orb_init();

      break;

    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;

      //Motors

    case MOTORS_INIT:

      usb_puts("calling motors_init\n");

      motors_init();

      break;

    case MOTOR1_SET:

      new_movement_command_received = 1;

      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:

      new_movement_command_received = 1;

      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:

      new_movement_command_received = 1;

      usb_puts("calling motors_off\n");

      motors_off();

      break;

    case MOVE:

      new_movement_command_received = 1;

      /* format for move: left direction, left velocity, right direction, right velocity */

      sprintf(buf, "calling move\n");

      usb_puts(buf);

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

      motor2_set(args[2], args[3]);

      /* Loop and update position for virtual wall. */

      move_routine();

      break;

    case PRINTF:

      usb_puts((char*)pkt->data);

      break;

    case KILL_ROBOT:

      new_movement_command_received = 1;

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

  }

}