Zumo Robot 自動走行アルゴリズム

超音波センサを使った自動走行アルゴリズムを考えました。

超音波センサを幾つ使うかによってアルゴリズムは変わると思います。
今回は、超音波センサを１つ使ったときのアルゴリズムを考えました。
できれば超音波センサを前用、右用、左用と３つ使いたかったのですが、小さいサイズのブレッドボードが1つしか無かったので超音波センサを１つとしました。
その姿は前回の記事にも出していますが、コレです。
デバッグなので、アイスホッケーのパックの上に載せて車輪の回転ができるようにしています。

さて、アルゴリズムはどうしますかね。

１つの超音波センサの活用法

１つしか超音波センサがないので、方法は２つ。
1つの方法は、前だけを測定し、障害物があった時には左右のどちらか決めた方向に姿勢を90度変えて、そこで同じように測定し、障害物が無ければ直進し、障害物があば同じように姿勢を90度変えて測定を行うということを繰り返す。
これが一番簡単な方法とは思いますが、この方法では部屋の壁沿いに走行しそうなのでイマイチな感じがします。
90度の姿勢変更を例えば45度とかにしてやってみても良いかも知れませんが壁沿い走行の可能性は残ります。
2つ目の方法は、その場で3方向に向けた距離を測り、一番良い方向に姿勢を変えそうこうするという方法です。

止まっている位置で正面の距離を測り、姿勢を右に少し変えて距離を測り、それから左に少し姿勢を変えて距離を測り、一番距離が長い方向に姿勢を変えて走行する。
この方法であれば、その場その場で障害物から一番距離が離れている方向に姿勢を変えることになり、自動走行っぽく走れる可能性はあります。

必要なコードは？

このアルゴリズムに必要なコードは以下の４つ。
１）Zumo Robotoのモータを動かすためのコード
　　これはZumoMotor..hというライブラリがあるのでこれを使います。
２）超音波センサHC-SR04で距離を測定するためのコード
　　これはNewPing.hというライブラリがあるのでこれを使います。
３）正面、右、左を向いて距離を測るためのコード
　　これは自分で作ります。
４）測った3つの距離から進行方向を決め、その方向に姿勢を変えるためのコード
　　これも自分で作ります。
大まかには以上になります。

正面、右、左を向いて距離を測るためのコード

このコードがコチラになります。
まずは止まって、正面で測定し、その後右方向に向きを変えて測定し、その後正面を通り越して左方向に向きを変えて測定し、正面に向きを戻して終了です。

測った3つの距離から進行方向を決め、その方向に姿勢を変えるためのコード

このコードがコチラになります。
正面に障害物が無いと判断した時は前進します。
正面に障害物があり、右と左の距離を比較し、右の距離が大きいときは右折し、
正面に障害物があり、右と左の距離を比較し、左の距離が大きいときは左折します。

まずはこれで動かしてみます。
動いている動画を撮ろうと思いますので、続きはまた明日。
今日はここまで。
では、また。