lines.cpp

/*
  連続直線への追従
  Satofumi KAMIMURA
  $Id$
*/

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


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

    // 走行経路
    std::deque<VXV::Position> lines;
    lines.push_back(VXV::Position(0, 0, deg(-45)));
    lines.push_back(VXV::Position(1000, -1000, deg(90)));
    lines.push_back(VXV::Position(1000, 1000, deg(45)));
    lines.push_back(VXV::Position(2000, 2000, deg(-90)));
    lines.push_back(VXV::Position(2000, 0, deg(180)));
    lines.push_back(VXV::Position(0, 0, deg(180)));

    // 走行経路の表示
    std::deque<VXV::Grid3D> draw_lines;
    draw_lines.assign(lines.begin(), lines.end());
    vmonitor::drawContLine(draw_lines, White);

    // 実際の走行処理
    int follow_r = run.getCurveRadius();
    run.followLine(lines.front());
    while (lines.size() > 0) {
      VXV::followLinesUpdate(run, lines, follow_r);
      VXV::Delay(100);          // Do something
    }
    run.stop();
    VXV::Delay(1000);

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