judgeCtrl.cpp

/*
  判定コマンド実装
  Satofumi KAMIMURA
  $Id$
*/

#include "runCtrl.h"


bool RunCtrl::isStable(void) {
  int stable = 0;
  if (recvStableValue(&stable) < 0) {
    throw RunCtrl_Exception("Transmit fail: in recvStableValue()");
  }
  return (stable == 0) ? false : true;
}


int RunCtrl::getLengthToGrid(const VXV::Grid& grid,
                             const CoordinateCtrl* crd) {
  VXV::Position position = getRunPosition(crd);
  int x_diff = position.x - grid.x;
  int y_diff = position.y - grid.y;

  return (int)sqrt((float)((x_diff * x_diff) + (y_diff * y_diff)));
}


int RunCtrl::getLengthToBody(const VXV::Grid& grid,
                             const CoordinateCtrl* crd) {
  return getLengthToGrid(grid, crd);
}


int RunCtrl::getLengthToLine(const VXV::Position& line,
                             const CoordinateCtrl* crd) {

  VXV::Position position = getRunPosition(crd);
  int diff_x = position.x - line.x;
  int diff_y = position.y - line.y;
  double radian = line.zt.to_rad();

  return (int)(diff_x * cos(radian) + diff_y * sin(radian));
}


VXV::Direction RunCtrl::getAngleToDirection(const VXV::Direction& t,
                                            const CoordinateCtrl* crd) {

  return getRunPosition(crd).zt - t;
}


int RunCtrl::getStraightVelDiff(int mm_vel) {
  return getStraightVel() - mm_vel;
}


VXV::Direction RunCtrl::getRotateVelDiff(const VXV::Direction& t) {
  return getRotateVel() - t;
}