run_beginner_task_05.cpp

/*
  図のように、半径１[m]の 1/4円弧の扇形の軌跡を描くようにビーゴを走行させなさい
  Satofumi KAMIMURA
  $Id$
*/

#include <mRunCtrl.h>
#include <vutils.h>
#include <stdio.h>

using namespace VXV;


static void stopAndTurn(mRunCtrl& run, const VXV::Position& target) {
  while (abs(run.getLengthToLine(target)) > 300) {
    VXV::Delay(100);
  }
  run.stopToLine(target);
  waitStable(run, 100);

  // この課題では、経路の切り替え毎に回転が必要なので、ここで回転させる
  run.rotateAngle(deg(-90));
  waitStable(run, 100);
}


int main(int argc, char *argv[]) {
  try {
    mRunCtrl run;
    if (initConnection(&run, argc, argv) < 0) {
      exit(1);
    }
    vmonitor::show();

    // 最初の直線
    VXV::Position target(1000, 0, deg(0));
    run.followLine(target);
    stopAndTurn(run, target);

    // 円弧
    run.followCircle(VXV::Grid(0, 0), 1000);
    stopAndTurn(run, VXV::Position(0, -1000, deg(180)));

    // ２番目の直線
    target = VXV::Position(0, 0, deg(90));
    run.followLine(target);
    stopAndTurn(run, target);

    VXV::Delay(1000);
  } catch (std::exception& e) {
    printf("exception: %s\n", e.what());
  }
  return 0;
}