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 unsigned target_x, target_y;

static volatile int updated_robot_pos_ready;

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;

typedef enum {MOVING, MOVING_TO_POSITION, NO_COMMAND} ColonetRobotCommandState;

static volatile ColonetRobotCommandState robot_state;

static volatile int virtual_wall_up_available, virtual_wall_low_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;

}

static int virtual_wall_available() {

  return virtual_wall_up_available && virtual_wall_low_available;

}

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

  robot_lost = 1;

  virtual_wall_up_available = 0;

  virtual_wall_low_available = 0;

  virtual_wall.up_x = 0;

  virtual_wall.up_y = 0;

  virtual_wall.low_x = 0;

  virtual_wall.low_y = 0;

  robot_state = NO_COMMAND;

  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;

}

static void report_arrived() {

  //usb_puts("requesting_abs_position\n");

  ColonetRobotServerPacket pkt;

  pkt.client_id = -1; //global

  pkt.msg_code = ROBOT_REPORT_ARRIVED_AT_POSITION;

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

}

void request_abs_position() {

  //usb_puts("requesting_abs_position\n");

  ColonetRobotServerPacket pkt;

  pkt.client_id = -1; //global

  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 int inside_virtual_wall(int x, int y) {

  if (virtual_wall_available()) {

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

        || robot_y > virtual_wall.low_y) {

      return 0;

    } else {

      return 1;

    }

  } else {

    return 1;

  }

}

/**

 * Main loop which responds to commands.

 */

void colonet_run(void (*step)(void)) {

  char buf[80];

  while (1) {

    wl_do();

    request_abs_position();

    step();

    switch (robot_state) {

    case NO_COMMAND:

      break;

    case MOVING:

      if (robot_lost) {

        motors_off();

        robot_state = NO_COMMAND;

      } else {

        if (updated_robot_pos_ready) {

          if (!inside_virtual_wall(robot_x, robot_y)) {

            usb_puts("outside of boundary\n");

            motors_off();

            robot_state = NO_COMMAND;

          }

          updated_robot_pos_ready = 0;

        }

      }

      break;

    case MOVING_TO_POSITION:

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

      if (robot_lost) {

        motors_off();

        robot_state = NO_COMMAND;

      } else {

        if (updated_robot_pos_ready) {

          updated_robot_pos_ready = 0;

          if (!inside_virtual_wall(robot_x, robot_y)) {

            usb_puts("outside of boundary\n");

            motors_off();

            robot_state = NO_COMMAND;

          } else {

            int dist = 0;

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

              // Reached destination.

              motors_off();

              robot_state = NO_COMMAND;

              // Notify server that robot arrived.

              report_arrived();

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

              }

            }

          }

        }

      }

      break;

    }

  }

}

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

      robot_lost = 1;

      usb_puts("lost\n");

      motors_off();

      break;

    case SERVER_CLEAR_VIRTUAL_WALL:

      virtual_wall_up_available = 0;

      virtual_wall_low_available = 0;

      break;

    case SERVER_REPORT_VIRTUAL_WALL_UPPER:

      virtual_wall.up_x = int_args[0];

      virtual_wall.up_y = int_args[1];

      virtual_wall_up_available = 1;

      sprintf(buf, "%d %d\n", virtual_wall.up_x, virtual_wall.up_y);

      usb_puts(buf);

      break;

    case SERVER_REPORT_VIRTUAL_WALL_LOWER:

      virtual_wall.low_x = int_args[0];

      virtual_wall.low_y = int_args[1];

      sprintf(buf, "%d %d\n", virtual_wall.low_x, virtual_wall.low_y);

      usb_puts(buf);

      virtual_wall_low_available = 1;

      break;

    case SERVER_REPORT_POSITION_TO_ROBOT:

      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;

      robot_lost = 0;

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

      //usb_puts(buf);

      break;

    case MOVE_TO_ABSOLUTE_POSITION:

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

                        usb_puts(buf);

      target_x = (unsigned)int_args[0];

      target_y = (unsigned)int_args[1];

      robot_state = MOVING_TO_POSITION;

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

      robot_state = NO_COMMAND;

      usb_puts("calling motors_off\n");

      motors_off();

      break;

    case MOVE:

      if (args[1] == 0 && args[3] == 0) {

        motors_off();

        robot_state = MOVING;

        usb_puts("stopped\n");

      } else {

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

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

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

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

        robot_state = MOVING;

      }

      break;

    case PRINTF:

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

      break;

    case KILL_ROBOT:

      robot_state = NO_COMMAND;

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

  }

}