moveCtrl.cpp

/*
  移動コマンド実装
  Satofumi KAMIMURA
  $Id$
*/

#include "runCtrl.h"


int RunCtrl::to_div16(const VXV::Direction& t) {
  return static_cast<int>(0x10000 * t.to_rad() / (2.0 * M_PI));
}


const VXV::Position RunCtrl::getRunCmdPos(const CoordinateCtrl* crd,
                                          const VXV::Position& position) {
  VXV::Position3D run_pos = position;

  VXV::Direction rotate = VXV::Direction() - local_offset.zt;
  VXV::Position3D offset;
  convert(offset, local_offset,
          createConvertMatrix(VXV::Position(0, 0, rotate)));
  run_pos -= VXV::Position(local_offset.x, local_offset.y, VXV::Direction());
  convertWithAngle(run_pos, run_pos, VXV::Position(0, 0, rotate));

  if (crd == VXV::GL) {
    run_pos = getRootCrd()->getCrdPosition(this, run_pos);
  } else {
    run_pos = crd->getCrdPosition(this, run_pos);
  }
  return run_pos;
}


void RunCtrl::stop(void) {
  runCommand_t& cmd = state_stack.front().command;
  if (sendStop(cmd.send_command, &cmd.send_command_size) < 0) {
    cmd.send_command_size = 0;
    throw RunCtrl_Exception("Transmit fail: in sendStop()");
  }
}


void RunCtrl::followLine(const VXV::Position& position,
                         const CoordinateCtrl* crd) {
  VXV::Position run_pos = getRunCmdPos(crd, position);

  runCommand_t& cmd = state_stack.front().command;
  if (sendFollowLine(run_pos.x, run_pos.y, to_div16(run_pos.zt),
                     cmd.send_command, &cmd.send_command_size) < 0) {
    cmd.send_command_size = 0;
    throw RunCtrl_Exception("Transmit fail: in sendFollowLine()");
  }
  cmd.position = position;
  cmd.crd = crd;
}



void RunCtrl::followCircle(const VXV::Grid& center, int radius,
                           const CoordinateCtrl* crd) {
  int set_radius = (radius != 0) ? radius : 1;
  VXV::Position run_pos =
    getRunCmdPos(crd, VXV::Position(center.x, center.y, VXV::Direction()));

  int follow_r = state_stack.front().params.follow_r;
  if (abs(radius) < follow_r) {
    setCurveRadius(abs(radius));
  }
  runCommand_t& cmd = state_stack.front().command;
  if (sendFollowCircle(run_pos.x, run_pos.y, set_radius,
                       cmd.send_command, &cmd.send_command_size) < 0) {
    cmd.send_command_size = 0;
    throw RunCtrl_Exception("Transmit fail: in sendFollowCircle()");
  }
  setCurveRadius(follow_r);

  cmd.position = VXV::Position(center.x, center.y, VXV::Direction());
  cmd.crd = crd;
}


void RunCtrl::followCircleOnTangent(const VXV::Position& position, int radius,
                                    const CoordinateCtrl* crd) {

  double t_add = (radius > 0) ? +M_PI/2.0 : -M_PI/2.0;
  double radian = position.zt.to_rad() + t_add + M_PI;
  VXV::Grid center(position.x + static_cast<int>(abs(radius) * cos(radian)),
                   position.y + static_cast<int>(abs(radius) * sin(radian)));

  return followCircle(center, radius, crd);
}


void RunCtrl::stopToLine(const VXV::Position& position,
                         const CoordinateCtrl* crd) {
  VXV::Position run_pos = getRunCmdPos(crd, position);

  runCommand_t& cmd = state_stack.front().command;
  if (sendStopToLine(run_pos.x, run_pos.y, to_div16(run_pos.zt),
                     cmd.send_command, &cmd.send_command_size) < 0) {
    cmd.send_command_size = 0;
    throw RunCtrl_Exception("Transmit fail: in sendStopToLine()");
  }
  cmd.position = position;
  cmd.crd = crd;
}


void RunCtrl::rotateToDirection(const VXV::Direction& direction,
                                const CoordinateCtrl* crd) {
  VXV::Position run_pos = getRunCmdPos(crd, VXV::Position(0, 0, direction));

  runCommand_t& cmd = state_stack.front().command;
  if (sendTurnToDirection(to_div16(run_pos.zt),
                          cmd.send_command, &cmd.send_command_size) < 0) {
    cmd.send_command_size = 0;
    throw RunCtrl_Exception("Transmit fail: in sendTurnToDirection()");
  }
  cmd.position = VXV::Position(0, 0, direction);
  cmd.crd = crd;
}


void RunCtrl::rotateAngle(const VXV::Direction& direction) {

  runCommand_t& cmd = state_stack.front().command;
  int div16 = static_cast<int>(0x10000 * direction.getInnerRadian()
                               / (2.0 * M_PI));
  if (sendRotateAngle(div16, cmd.send_command, &cmd.send_command_size) < 0) {
    cmd.send_command_size = 0;
    throw RunCtrl_Exception("Transmit fail: in sendRotateAngle()");
  }
}


void RunCtrl::spin(const VXV::Direction& velocity) {

  runCommand_t& cmd = state_stack.front().command;
  if (sendSpin(to_div16(velocity),
               cmd.send_command, &cmd.send_command_size) < 0) {
    cmd.send_command_size = 0;
    throw RunCtrl_Exception("Transmit fail: in sendSpin()");
  }
}


void RunCtrl::lastMoveCommand(const CoordinateCtrl* crd) {

  // 移動コマンドの再発行
  runCommand_t& cmd = state_stack.front().command;
  if (cmd.send_command_size <= 0) {
    return;
  }
  long last_command_unique_id =
    getPacketUniqueId((unsigned char*)cmd.send_command);
  if (sendLastMoveCommand(last_command_unique_id,
                          cmd.send_command, cmd.send_command_size) < 0) {
    cmd.send_command_size = 0;
    throw RunCtrl_Exception("Transmit fail: in sendLastMoveCommand()");
  }

  // 位置の再評価
  if (crd_auto) {
    sendUpdatedPosition(cmd.crd, cmd.position);
  }
}