run_beginner_task_03.cpp

/*
  図のように、ビーゴを１[m]前進させて停止し、右90[degree]回転して停止、
  さらに１[m]前進して停止させなさい。
  この時、ロボットは急に止まれないことを考慮して、プログラムを工夫しなさい。
  Satofumi KAMIMURA
  $Id$
*/

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

using namespace VXV;


static void stopToTarget(mRunCtrl& run, const VXV::Position& target) {
  run.followLine(target);
  while (abs(run.getLengthToLine(target)) > 300) {
    // 条件が成立するまでポーリング。しばらく移動するのを待つため sleep する
    VXV::Delay(100);
  }
  run.stopToLine(target);
  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));
    stopToTarget(run, target);

    // 回転
    run.rotateToDirection(deg(-90));
    waitStable(run, 100);

    // 直進
    target = VXV::Position(1000, -1000, deg(-90));
    stopToTarget(run, target);

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