/*
  デバイスの直接制御実装
  Satofumi KAMIMURA
  $Id: directCmdCtrl.cpp 286 2008-10-20 09:40:22Z satofumi $
*/

#include "runCtrl.h"

using namespace VXV;


void RunCtrl::servoCtrl(bool on) {
  if (sendServoCtrl(on ? 1 : 0)) {
    throw RunCtrl_Exception("sendServoCtrl()");
  }
}


/*!
  \brief モータの PWM を直接指定

  \param id [i] モータID
  \param duty [i] PWM比 [0, 255]
  \retval *this
*/
void RunCtrl::setMotorPwm(int id, unsigned char duty) {
  unsigned char duty_array[1] = { duty };
  if (sendMotorDuty(id, duty_array, 1) < 0) {
    throw RunCtrl_Exception("sendMotorDuty()");
  }
}

void RunCtrl::setMotorPwm(int id, unsigned char duty[], int num) {
  if (sendMotorDuty(id, duty, num) < 0) {
    throw RunCtrl_Exception("sendMotorDuty()");
  }
}


/*!
  \brief モータの制御モードを直接指定

  指定可能なモードは、FREE, CW, CWW のいずれか

  \param id [i] モータID
  \param mode [i] モータのモード
  \retval *this
*/
void RunCtrl::setMotorMode(int id, unsigned char mode) {
  unsigned char mode_array[1] = { mode };
  if (sendMotorMode(id, mode_array, 1) < 0) {
    throw RunCtrl_Exception("sendMotorMode()");
  }
}

void RunCtrl::setMotorMode(int id, unsigned char mode[], int num) {
  if (sendMotorMode(id, mode, num) < 0) {
    throw RunCtrl_Exception("sendMotorMode()");
  }
}


/*!
  \brief エンコーダ速度の取得

  1 [msec]あたりのエンコーダ値の変化量を取得する

  \param id [i] エンコーダID
  \param cnt [o] エンコーダ値の変位 [1]
  \retval *this
*/
void RunCtrl::getEncoderVel(int id, int *cnt) {
  int cnt_array[1];
  if (recvEncoderDiff(id, cnt_array, 1) < 0) {
    throw RunCtrl_Exception("recvEncoderDiff()");
  }
  *cnt = cnt_array[0];
}

void RunCtrl::getEncoderVel(int id, int cnt[], int num) {
  if (recvEncoderDiff(id, cnt, num) < 0) {
    throw RunCtrl_Exception("recvEncoderDiff()");
  }
}


/*!
  \brief エンコーダカウンタ値の取得

  現時点でのエンコーダカウンタ値を取得

  \param id [i] エンコーダID
  \param cnt [o] エンコーダのカウンタ値 [1]
  \retval *this

  \attention 取得間隔によっては、エンコーダのオーバーフローを検出できない
*/
void RunCtrl::getEncoderValue(int id, unsigned short *cnt) {
  unsigned short cnt_array[1];
  if (recvEncoderValue(id, cnt_array, 1) < 0) {
    throw RunCtrl_Exception("recvEncoderValue()");
  }
  *cnt = cnt_array[0];
}

void RunCtrl::getEncoderValue(int id, unsigned short cnt[], int num) {
  if (recvEncoderValue(id, cnt, num) < 0) {
    throw RunCtrl_Exception("recvEncoderValue()");
  }
}


/*!
  \brief 車輪の移動速度を直接指定

  \param id [i] 車輪ID
  \param mm_vel [i] 車輪の移動速度 [mm/sec]
  \retval *this
*/
void RunCtrl::setWheelVel(int id, int mm_vel) {
  int mm_vel_array[1] = { mm_vel };
  if (sendWheelVel(id, mm_vel_array, 1) < 0) {
    throw RunCtrl_Exception("sendWheelVel()");
  }
}

void RunCtrl::setWheelVel(int id, int mm_vel[], int num) {
  if (sendWheelVel(id, mm_vel, num) < 0) {
    throw RunCtrl_Exception("sendWheelVel()");
  }
}
