runParameterCtrl.cpp

/*
  走行制御パラメータ更新
  Satofumi KAMIMURA
  $Id$
*/

#include "runCtrl.h"


void RunCtrl::setStraightRefVel(int mm_sec) {
  state_stack.front().params.straight_ref_vel = mm_sec;
  if (sendStraightRefVel(mm_sec) < 0) {
    throw RunCtrl_Exception("Transmit fail: in sendStraightRefVel()");
  }
}


void RunCtrl::setStraightRefAcc(int mm_acc) {
  state_stack.front().params.straight_ref_acc = mm_acc;
  if (sendStraightRefAcc(mm_acc) < 0) {
    throw RunCtrl_Exception("Transmit fail: in sendStraightRefAcc()");
  }
}


void RunCtrl::setRotateRefVel(const VXV::Direction& rotate_vel) {
  state_stack.front().params.rotate_ref_vel = to_div16(rotate_vel);
  if (sendRotateRefVel(to_div16(rotate_vel)) < 0) {
    throw RunCtrl_Exception("Transmit fail: in sendRotateRefVel()");
  }
}


void RunCtrl::setRotateRefAcc(const VXV::Direction& rotate_acc) {
  state_stack.front().params.rotate_ref_acc = to_div16(rotate_acc);
  if (sendRotateRefAcc(to_div16(rotate_acc)) < 0) {
    throw RunCtrl_Exception("Transmit fail: in sendRotateRefAcc()");
  }
}


void RunCtrl::setCurveRadius(int mm) {
  state_stack.front().params.follow_r = mm;
  if (sendCurveRadius(mm) < 0) {
    throw RunCtrl_Exception("Transmit fail: in sendCurveRadius()");
  }
}


int RunCtrl::getStraightVel(void) {
  int straight_vel = 0;
  if (recvStraightVel(&straight_vel) < 0) {
    throw RunCtrl_Exception("Transmit fail: in recvStraightVel()");
  }
  return straight_vel;
}


VXV::Direction RunCtrl::getRotateVel(void) {
  int rotate_vel = 0;
  if (recvRotateVel(&rotate_vel) < 0) {
    throw RunCtrl_Exception("getRotateVel()");
  }
  return VXV::Direction::rad(2.0 * M_PI * rotate_vel / 0x10000);
}


int RunCtrl::getCurveRadius(void) {
  return state_stack.front().params.follow_r;
}