urg_beginner_task_07.cpp

/*
  ビーゴを直進させ、障害物が前方 60[cm]以内に入ったら測域センサで感知し、
  ビーゴを停止させなさい。
  また、障害物がなくなると再び直進するようにしないさい。
  Satofumi KAMIMURA
  $Id$
*/

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

using namespace VXV;

enum {
  BodyWidth = 330,              // ビーゴの筐体幅
};


// 筐体の前方にある物体との最も近い距離を返す
static int getFrontLength(std::vector<VXV::Grid3D>& points) {

  int min_length = -1;
  for (std::vector<Grid3D>::iterator it = points.begin();
       it != points.end(); ++it) {
    if ((abs(it->y) < (BodyWidth + 100)/2) && (it->x > 0)) {
      if ((min_length < 0) || (min_length > it->x)) {
        min_length = it->x;
      }
    }
  }
  return min_length;
}


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

    vmonitor::show();

    run.followLine(VXV::Position(0, 0, deg(0)));
    //run.followCircle(Grid(0, 0), 1000);

    unsigned long beginTicks = VXV::GetTicks();
    bool moving = true;
    while (VXV::GetTicks() - beginTicks < 30000) {
      urg.capture();
      urg.convert();

      int length = getFrontLength(urg.crd_points);
      if ((length > 0) && (length < 600)) {
        if (moving) {
          // 直前の移動コマンドを待避してから停止コマンドを発行
          run.push_runState();
          run.stop();
          run.pop_runState();
          moving = false;
        }
      } else if (!moving) {
        // 移動を再開
        run.lastMoveCommand();
        moving = true;
      }
    }

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