runCoordinate.cpp

/*
  自己位置取得コマンド実装
  Satofumi KAMIMURA
  $Id$
*/

#include "runCtrl.h"


VXV::Position3D RunCtrl::getLocalPosition(void) {
  int x, y;
  long div16;
  if (recvGetPosition(&x, &y, &div16) < 0) {
    throw RunCtrl_Exception("Transmit fail: in recvGetPosition()");
  }

  crd_ticks = getModuleTicks(updateTicksDiff(tbl->sec * 1000 + tbl->msec));
  crd_position =
    VXV::Position(x, y, VXV::Direction::rad(2.0*M_PI* div16 / 0x10000));
  convertWithAngle(crd_position, crd_position, local_offset);

  return crd_position;
}


VXV::Position RunCtrl::getRunPosition(const CoordinateCtrl* crd) {
  return getCrdPosition(crd, getLocalPosition());
}


void RunCtrl::adjustRunPosition(const VXV::Position& position,
                                const CoordinateCtrl* crd) {

  local_offset = VXV::Position();
  VXV::Position actual = getRunPosition();
  local_offset = position - actual;

  if (crd_auto) {
    VXV::Position send_offset = getCrdPosition(crd, local_offset);
    sendUpdatedPosition(this, send_offset);
  }
}


void RunCtrl::coordinateUpdateDetect(bool on) {
  crd_auto = on;
}


void RunCtrl::sendUpdatedPosition(const CoordinateCtrl* crd,
                                  const VXV::Position& position) {
  VXV::Position run_pos = getCrdPosition(crd, position);
  if (sendChangeCoordinateOffset(run_pos.x, run_pos.y,
                                 static_cast<int>(0x10000 * run_pos.zt.to_rad()
                                                  / (2.0 * M_PI))) < 0) {
    throw RunCtrl_Exception("Transmit fail: in sendChangeCoordinateOffset()");
  }
}