crdUpdate.cpp

/*
  座標系の位置更新の影響について
  Satofumi KAMIMURA
  $Id$
*/

#if !MONITOR
#include <runCtrl.h>
#else
#include <mRunCtrl.h>
#endif
#include <typePrint.h>
#include <stdio.h>
#include <stdlib.h>

using namespace VXV;


static void waitStable(RunInterface& run, unsigned long poll_time) {
  while (!run.isStable()) {
    VXV::Delay(100);
  }
}


int main(int argc, char *argv[]) {
  try {
#if !MONITOR
    RunCtrl run;
#else
    mRunCtrl run;
    vmonitor::show();
#endif
    if (run.connect(argc, argv) < 0) {
      printf("RunCtrl::connect: %s\n", run.what());
      exit(1);
    }

    for (int i = 0; i < 2; ++i) {
      run.coordinateUpdateDetect(i);

      run.followLine(Position(0, 0, deg(0)));
      VXV::Delay(2000);

      // 座標系の更新
      run.adjustCrdPosition(Position(0, 250, deg(0)));
      std::cout << "crd adjust: y = " << run.getRunPosition().y << std::endl;
      waitStable(run, 100);
      std::cout << "    stable: y = " << run.getRunPosition().y << std::endl;

      // 自己位置の更新
      run.adjustRunPosition(Position(0, 250, deg(0)));
      std::cout << "run adjust: y = " << run.getRunPosition().y << std::endl;
      waitStable(run, 100);
      std::cout << "    stable: y = " << run.getRunPosition().y << std::endl;

      putchar('\n');
    }
    VXV::Delay(3000);

    run.stop();
    VXV::Delay(1000);

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