初級編

走行制御編

研究室で最初に与えられる課題を、走行制御ライブラリを用いて解いてみます。モニタライブラリを使ったプログラムになっています。モニタ機能が使えない方は、mRunCtrl 等を RunCtrl に置き換え、vmonitorshow() などをコメントアウトして下さい。

問題１

ビーゴを反時計回りに３回転させなさい、また、時計回りに３回転させなさい

解答

指定角度だけ回転するためのコマンドがあるので、それを用いればよい

/*
  ビーゴを反時計回りに３回転させなさい、また、時計回りに３回転させなさい
  Satofumi KAMIMURA
  $Id$
*/

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

using namespace VXV;


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

    run.rotateAngle(deg(360 * 3)); // 反時計回りの場合
    // run.rotateAngle(deg(-360 * 3)); // 時計回りの場合

    waitStable(run, 100);       // 回転が終わるまで待つ

    VXV::Delay(1000);           // vmonitor::show() のため。なくてもよい
  } catch (std::exception& e) {
    printf("exception: %s\n", e.what());
  }
  return 0;
}

問題２

ビーゴを １[m]前進させて停止させなさい、また、１[m]後進させて停止させなさい

解答

直線上で停止するコマンドを、１[m]先の位置に対して発行する。
後進するためには、目標位置を後方 １[m]に設定すればよい。

/*
  ビーゴを １[m]前進させて停止させなさい、また、１[m]後進させて停止させなさい
  Satofumi KAMIMURA
  $Id$
*/

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

using namespace VXV;


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

#if 1
    // 前進の場合
    run.stopToLine(VXV::Position(1000, 0, deg(0)));
    waitStable(run, 100);
#else
    // 後進の場合
    run.stopToLine(VXV::Position(-1000, 0, deg(0)));
    waitStable(run, 100);
#endif

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

ちなみに、後進させるためには、目標並進速度を負の値にしてから直線追従コマンドを発行する

/*
  後進する場合
  Satofumi KAMIMURA
  $Id$
*/

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

using namespace VXV;


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

    // 並進速度を負の値にしてから経路追従コマンドを発行
    run.setStraightRefVel(-300);
    run.followLine(VXV::Position(0, 0, deg(0)));
    VXV::Delay(3000);

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

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

問題３

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

run_task_03.png

解答

停止目標の直線に到達してから停止コマンドを発行すると、おそらくその位置を乗り越えるので、目標直線に近づいたら、直線上での停止コマンドを発行する。

/*
  図のように、ビーゴを１[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;
}

問題４

図のように、ビーゴをスタート地点から前方１[m]横方向に伸びる直線に追従させなさい。

  1. ２つの直進命令を用いて、直線 L1 を追従してから直線 L2 に追従
  2. １つの直進命令を用いて、直線 L2 に追従

run_task_04.png

解答

  1. 目標直線に近づいたら、次の直線を発行する
  2. いきなり目標の直線追従コマンドを発行する

むしろ、追従曲率を変化させて動作を確認するとよい

/*
  図のように、ビーゴをスタート地点から前方１[m]横方向に伸びる直線に追従させなさい。
  1) ２つの直進命令を用いて、直線 L1 を追従してから直線 L2 に追従
  2) １つの直進命令を用いて、直線 L2 に追従
  Satofumi KAMIMURA
  $Id$
*/

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

using namespace VXV;


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

#if 1
    // 1) の場合
    VXV::Position target(1000, 0, deg(0));
    run.followLine(target);
    while (abs(run.getLengthToLine(target)) < 300) {
      VXV::Delay(100);
    }
    run.followLine(VXV::Position(1000, 1000, deg(-90)));
#else
    // 2) の場合
    run.followLine(VXV::Position(1000, 1000, deg(-90)));
#endif

    waitStable(run, 100);
    VXV::Delay(2000);
    run.stop();

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

問題５

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

run_task_05.png

解答

直線、円弧、直線、という経路追従を行えばよい

/*
  図のように、半径１[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;
}


測域センサ編

以下の問題は本来、超音波用に出題された問題です、なので、いろいろ無理があるかもしれませんが、頑張ります。

問題６

ビーゴの前方に板を置き、測域センサがどのような値を受け取っているかを画面に表示させなさい。

解答

モニタ機能を用いて測定点を描画すればよい

/*
  ビーゴの前方に板を置き、測域センサがどのような値を受け取っているかを
  画面に表示させなさい。
  Satofumi KAMIMURA
  $Id$
*/

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

using namespace VXV;


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

    unsigned beginTicks = VXV::GetTicks();
    while (VXV::GetTicks() - beginTicks < 10000) {
      // 測定
      urg.capture();
      urg.convert();

      // 描画
      vmonitor::clear();
      vmonitor::drawPoints(urg.crd_points, Red);
    }

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

問題７

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

解答

問題の通りに実装すればよい。測定点の二次元展開には、urg.convert() を用いた

/*
  ビーゴを直進させ、障害物が前方 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;
}

問題８

２枚の板をそれぞれ図のような形に組み合わせ、それぞれについて測域センサで測距できる距離を確認しなさい。

urg_task_08.png

解答

課題６で作ったプログラムを、上記環境で眺めてみればよい。


応用問題

これも、本当は超音波センサを用いたサンプルですが、測域センサを用いて無理矢理に解きたいと思います。あと、問題の 9 〜 19, 22 は、どうでもよい問題なので省略しました。

問題20

測域センサを用いて、100[cm] x 100[cm] 程度の段ボール箱の周りを３周させるプログラムを書き、ビーゴで実行しなさい。

解答

問題21

測域センサを使って、立てた棒(太さ数cm)と板(幅数十cm)を判別するプログラムを作りなさい。なお、ビーゴは棒や板を横に見ながら直進するものとする。

解答

問題23

図のように、障害物を測域センサで検出し、障害物を見つけると避けて、目的地まで進むプログラムを作りなさい。

last_task_23.png

解答

Generated on Mon Apr 13 22:52:06 2009 by  doxygen 1.5.7.1