/*!
  \file
  \brief 直線追従サンプル

  \author Satofumi KAMIMURA

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


#include "qrk_main.h"
#include "SimulationWindow.h"
#include "mBeegoDrive.h"
#include "delay.h"
#include <QApplication>

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 0
  // モータを直接制御する
  cout << "setWheelVelocity()" << endl;
  run.setWheelVelocity(0, 300);
  run.setWheelVelocity(1, 300);
#endif

#if 0
  // その場での回転
  cout << "spin()" << endl;
  run.spin(deg(90));
  while (! run.isStable()) {
    delay(100);
  }
#endif

#if 0
  // 指定した方向を向く
  cout << "rotate()" << endl;
  run.rotate(deg(-90));

  while (! run.isStable()) {
    delay(100);
  }
#endif

#if 1
  // 直線追従
  cout << "followLine()" << endl;
  run.followLine(Position<long>(0, 0, deg(45)));
  delay(3000);
#endif

#if 0
  // 円弧追従
  cout << "followCircle()" << endl;
  run.followCircle(Point<long>(0, 0), 500);
  delay(6000);
#endif

#if 0
  // 直線上で停止
  cout << "stopLine()" << endl;
  run.stopLine(Position<long>(2000, 0, deg(0)));
  while (! run.isStable()) {
    delay(100);
  }
#endif

#if 0
  // 自己位置の更新テスト
  cout << run.position() << endl;
  run.changeRobotPosition(Position<long>(0, 0, deg(0)),
                          Position<long>(100, 0, deg(90)));
  cout << run.position() << endl;

  run.followLine(Position<long>(0, 0, deg(0)));
#endif

  delay(3000);

  Position<long> position = run.position();
  cout << position << endl;

  run.stop();

  delay(2000);

  return 0;
}
