ODE/ライントレーサ - bambooflow Note

ライントレーサ

高校のとき、ライントレーサというロボットを作ったことがあります。
それがプログラムを知ったきっかけなんですが、今思うと懐かしいです。

懐かしついでにODEでも作れないかと思ってがんばってみました。
ODEならお金かからないしね。
メモ書きです。

test_linetracer.zip

ライントレーサは線の上をたどるロボットです。

ロボットの中央の下にプレート（黄）があります。
そのプレートには、左右両側にセンサ（赤と緑）が付いています。
センサは白い線に反応します。（実機では赤外線センサが付いていて白い線の上では赤外線が反射することで白線の存在を感知します。）
センサが反応すると、対応したモータが止まるよう制御がかかります。

今回はシンプルなつくりにするために、センサは左右に1つずつです。
高校のときのやつは中央付近に2つ付いていてあわせて4つで制御していました。
十字路でうまく動かなくて苦労したのを今でも記憶に残っています。

説明（ポイント）

このプログラムのポイントは、白線の検出のところです。
白線の検出には、Rayを使用しています。
Rayの使い方は、レイによる距離測定を参照してください。

Rayを使うとその半直線に接触した物体を感知し、その距離を知ることができます。
今回は、物体の感知のために使用しました。

• nearCallback関数内

   // センサー情報を取得
   if (sensor_l) {
       if (o1 == sensor_l->geom || o2 == sensor_l->geom) {
           dContactGeom c;
           int numc = dCollide( o1, o2, 1, &c, sizeof(dContactGeom) );
           if (numc > 0) {
               if (o1 == w_line->geom || o2 == w_line->geom) {
                   hit_l = 1;
                   printf( "left sensor hit!! %f\n", c.depth );
               }
           }
           return;
       }
   }
   if (sensor_r) {
       if (o1 == sensor_r->geom || o2 == sensor_r->geom) {
           dContactGeom c;
           int numc = dCollide( o1, o2, 1, &c, sizeof(dContactGeom) );
           if (numc > 0) {
               if (o1 == w_line->geom || o2 == w_line->geom) {
                   hit_r = 1;
                   printf( "right sensor hit!! %f\n", c.depth );
               }
           }
           return;
       }
   }

センサとなるRayに白線が接触することで、hit_l(int型)もしくはhit_r(int型)に1を代入します。

"category"と"collide"ビットフィールド制御すればもう少しかっこよくかけるかもしれません。

• simLoop関数内

       // モータ制御
       if (hit_l == 0) {
           dJointSetHingeParam( dBodyGetJoint(car_wheel_l->body, 0), dParamVel, 10.0 );
       } else {
           dJointSetHingeParam( dBodyGetJoint(car_wheel_l->body, 0), dParamVel, -5.0 );
       }
       if (hit_r == 0) {
           dJointSetHingeParam( dBodyGetJoint(car_wheel_r->body, 0), dParamVel, 11.0 );
       } else {
           dJointSetHingeParam( dBodyGetJoint(car_wheel_r->body, 0), dParamVel, -5.0 );
       }

hit_lもしくは、hit_rが1となると、メインループ内でモータを止めるような制御を行っています。

ちょっと失敗したのは、ヒンジジョイントをグローバル変数で用意しなかったためにアクセスがちょっと複雑になってしまったところです。

まとめ

プログラムはべた書きでかなり格好わるいです。さらにごまかしで作ってしまったところもあるのでもうちょっとしっかりと作りたいかな。
オブジェクト指向（コンポーネント指向）っていうのをODEでも書きやすいとよいのですが。。。

あと、白線は今回はシンプルに四角形の形にしました。
でも本当は配列マップから任意のコースを作るような仕組みを作りたかったんですが根気が足りず途中で投げてしまいました。
気合があれば次こそは。。。