move_ctrl.c

/*
  移動コマンド
  Satofumi KAMIMURA
  $Id$
*/
#define C_RUNCTRL_SOURCE


#include "move_ctrl.h"
#include "coordinate_ctrl.h"
#include "commandCtrl.h"
#include "nodeAccess.h"
#include <math.h>


extern int run_getGLCoordinateOffset(int *offset_x, int *offset_y,
                                     int *offset_div16,
                                     int dest_id, int x, int y, int div16);


enum { SEND_COMMAND_SIZE = 256 };
static char send_command[SEND_COMMAND_SIZE];
static int send_command_size = 0;
static int cmd_crd_id = -1;
static int cmd_x = 0;
static int cmd_y = 0;
static int cmd_div16 = 0;


#define MOVE_CMD_FIRST \
  int gl_x, gl_y, gl_div16; \
  int ret_value; \
  if (crd_id == FS) { \
    int now_x, now_y, now_div16; \
    run_getBodyPos(GL, &now_x, &now_y, &now_div16); \
    run_setCoordinateParent(FS, GL, now_x, now_y, now_div16); \
  } \
  run_getGLCoordinateOffset(&gl_x, &gl_y, &gl_div16, crd_id, x, y, div16); \


void run_getLastCommandPosition(int *crd_id, int *x, int *y, int *div16) {
  *crd_id = cmd_crd_id;
  *x = cmd_x;
  *y = cmd_y;
  *div16 = cmd_div16;
}

int run_followLine(int crd_id, int x, int y, int div16) {

  MOVE_CMD_FIRST;
  ret_value = sendFollowLine(gl_x, gl_y, gl_div16,
                             send_command, &send_command_size);
  if (ret_value < 0) {
    send_command_size = 0;
  }
  cmd_crd_id = crd_id;
  cmd_x = x;
  cmd_y = y;
  cmd_div16 = div16;

  return ret_value;
}


int run_stopLine(int crd_id, int x, int y, int div16) {

  MOVE_CMD_FIRST;
  ret_value = sendStopToLine(gl_x, gl_y, gl_div16,
                            send_command, &send_command_size);
  if (ret_value < 0) {
    send_command_size = 0;
  }
  cmd_crd_id = crd_id;
  cmd_x = x;
  cmd_y = y;
  cmd_div16 = div16;

  return ret_value;
}


int run_followCircle(int crd_id, int x, int y, int r) {
  int div16 = 0;
  MOVE_CMD_FIRST;
  ret_value = sendFollowCircle(gl_x, gl_y, r,
                             send_command, &send_command_size);
  if (ret_value < 0) {
    send_command_size = 0;
  }
  cmd_crd_id = crd_id;
  cmd_x = x;
  cmd_y = y;
  cmd_div16 = div16;

  return ret_value;
}


int run_followCircleOnLine(int crd_id, int x, int y, int div16, int r) {

  int t_add = (r > 0) ? +(0x10000 >> 2) : -(0x10000 >> 2);
  double radian = (2.0 * M_PI * (div16 + t_add) / 65536.0) + M_PI;
  int run_x = x + (int)(fabs(r) * cos(radian));
  int run_y = y + (int)(fabs(r) * sin(radian));

  return run_followCircle(crd_id, run_x, run_y, r);
}


int run_rotateAngle(int div16) {
  int ret_value = sendRotateAngle(div16, send_command, &send_command_size);
  if (ret_value < 0) {
    send_command_size = 0;
  }

  return ret_value;
}


int run_stopAngle(int crd_id, int div16) {
  int x = 0, y = 0;
  MOVE_CMD_FIRST;
  ret_value = sendTurnToDirection(gl_div16, send_command, &send_command_size);
  if (ret_value < 0) {
    send_command_size = 0;
  }
  cmd_crd_id = crd_id;
  cmd_div16 = div16;

  return  ret_value;
}


int run_spin(int div16) {
  int x = 0, y = 0;
  int crd_id = GL;
  MOVE_CMD_FIRST;
  ret_value = sendSpin(gl_div16, send_command, &send_command_size);
  if (ret_value < 0) {
    send_command_size = 0;
  }
  return  ret_value;
}


int run_stop(void) {
  return sendStop(send_command, &send_command_size);
}


int run_lastMovedCommand(void) {
  int last_command_unique_id;
  if (send_command_size <= 0) {
    return -1;
  }
  last_command_unique_id = getPacketUniqueId((unsigned char*)send_command);
  return sendLastMoveCommand(last_command_unique_id,
                             send_command, send_command_size);
}


int run_restartServoCtrl(void) {
  return sendServoCtrl(1);
}


int run_stopServoCtrl(void) {
  return sendServoCtrl(0);
}