/*!
  \example cpp/move_straight.cpp

  \brief 「まっすぐ走って止まる」サンプル

  \author Satofumi KAMIMURA

  $Id: move_straight.cpp 1688 2010-02-11 23:10:16Z satofumi $
*/

#include <qrk_main.h>
#include <mBeegoDrive.h>
#include <delay.h>

using namespace qrk;
using namespace std;


int main(int argc, char *argv[])
{
  static_cast<void>(argc);
  static_cast<void>(argv);

  // 接続
  mBeegoDrive run;
  if (! run.connect("/dev/ttyUSB0")) {
    cout << "BeegoDrive::connect: " << run.what() << endl;
    return 1;
  }

#if 1
  //////////////////////////////////////////////////////////////////////
  // 車輪速を指定してまっすぐ走り、一定時間後に停止させる

  for (int i = 0; i < 2; ++i) {
    // 300 [mm/sec] で車輪を回転させる
    run.setWheelVelocity(i, 300);
  }

  // ２秒だけ状態を維持
  delay(2000);

  // 停止を指示
  run.stop();

  // stop() は移動を開始するだけなので、完全に停止するまで少し待つ
  delay(2000);
#endif

  //////////////////////////////////////////////////////////////////////
  // 止まる位置が指定できる直線追従コマンドを使う

  // (1000, 0) の位置を通り、0 [deg] の方向に垂直な直線上で停止させる
  run.stopLine(1000, 0, deg(0));
  while (! run.isStable()) {
    // コマンドが安定状態になるまで待つ
    // stopLine() の場合、指定位置で停止したときに、安定状態になる
    delay(100);
  }

  return 0;
}
