move_ctrl.c

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

  static int fd な変数が run_ctrl.c で宣言されているので、
  このソースコードは、run_ctrl.c にて include することでコンパイルしている
*/


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


static position_t getRunCmdPos(int crdId, int x, int y, direction_t zt) {
  position_t run_pos;
  run_pos.x = x;
  run_pos.y = y;
  run_pos.zt = zt;

  direction_t rotate;
  rotate.div16 = -local_offset.zt.div16;
  position_t 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 (crdId == GL_ID) {
    run_pos = getRootCrd()->getCrdPosition(this, run_pos);
  } else {
    run_pos = crd->getCrdPosition(this, run_pos);
  }
  return run_pos;
}
#endif


int run_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()");
    return -1;
  }
  return 0;
}


int run_followLine(int x, int y, direction_t angle, int crdId) {
  //VXV::Position run_pos = getRunCmdPos(crd, position);
  position_t run_pos = getRunCmdPos(crdId, x, y, angle);
  //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;
}


#if 0 // !!! 以降は保留中
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);
  }
}
#endif