迷路探索ロボット (Maze path search robot) その５（プログラム編）
参考プログラムについて
迷路ロボットをweb検索すると多くの製作例や解説がヒットします。 私が参考にさせて頂いた記事は
MJRoBot（Marcelo Rovai）さんが6年前に発表されたMaze Solver Robotです。
プログラムを公開して頂いたMarcelo Rovaiさんに感謝申し上げます。
このロボットの動作やプログラムが詳しく解説されているので是非ご覧下さい。
上記のリンクを開き記事を最後まで読まれた後、興味のある方はページの最後の方でプログラムが
ダウンロード出来るので実行して下さい。
zipファイルを解凍すると次の2種類のArduinoプログラムコードが見れますが、どちらも基本機能は同じです。
①smart_MJRoBot_Maze_Solve_Left_Hand_Algorithm
②smart_MJRoBot_Maze_Solve_Phase_2_with__Optimization
私は②のsmart_MJRoBot_Maze_Solve_Phase_2_with__Optimizationを参考にました。
ファイルを開くと次のように6個のファンクションに機能がまとめられているのが解ります。
①smart_MJRoBot_Maze_Solve_Phase_2_with__Optimization：メインプログラム
②generalFunctions：サーボモータとBluetoothのコントロール関係（不使用）
③mazeFunctions：迷路探索記録関係
④motorFuntions：モータ関係
⑤sensorFuntions：センサー関係
⑥robotDefines.h：ヘッダファイル 関数設定関係（メインプログラムに統合）
私はハードウエアの違いに合わせ、プログラムを変更して使わせて頂きました。
ハード的な相違点
モータ ： ロータリー式RCサーボモータ ⇒ 1回転200ステップのステッピングモータ
モータ制御 ： サーボモータの制御 ⇒ ステッピングモータの制御
(ステッピングモータの制御が正確なのでPID制御を不使用）
フォトセンサー ： センサーユニット7個使用 ⇒ センサーモジュール5個を使用した基板を手組。
LO、RO（左右外側センサー：コースの停止ラインや各分岐の状況を読み取る）
LI、RI（左右内側センサー：ストレート走行時のラインに対する左右へのブレを読み取る）
CT（中央センサー：常時ラインを監視、他のセンサー情報と組み合わせてアクションを実行する）
（フォトセンサーのアナログ出力をAIではなくDIに入力し、デジタル入力のスレッショルド（しきい値）により、
コース面の白色とテープの黒色を 識別する方法を使いました）
Bluetooth ： Arduino Nano + HC-06Bluetoothモジュール ⇒ Bluetoothを不使用
迷路探索ロボットのプログラム
注意：ハードウエアの仕様が変わるとプログラムを修正しないと使えません。
（長文になるので各ファンクションをクリックすると別ページにてプログラムコードが開きます）。
１）main_Function
ヘッダーファイルを使わずに統合しました。
注意：メインプログラムのディレクトリー下に各ファンクションのプログラムを置きます。
主な設定箇所
①ライブラリーのダウンロードとインストール
1. #include "BasicStepperDriver.h"
2. #include "SyncDriver.h"
ここでは、BasicStepperDriver.h及びSyncDriver.hを使ってスッテプモータを制御します。
ます、ステップモータドライバーのライブラリーを次のリンク内のアイコンCode▼からZipファイルをダウンロードし、
Arduino IDEのスケッチから⇒ライブラリーのインクルード⇒Zip形式のライブラリーをインストールします。
GitHubからライブラリーをダウンロード
②ハードウエア―の定義
12. #define MOTOR_STEPS 200 // 1回転のステップ数を設定
13. #define L_Motor_RPM 40 // 左モータの1分間の回転数を設定：走行速が決定します。
14. #define R_Motor_RPM 40 // 右モータの1分間の回転数を設定：走行速が決定します。
15. #define MICROSTEPS 16 // モータドライバーA4988のマイクロステップ数の設定
16. #define ENB_Pin 19 // ESB32のGPIO19をイネーブルに設定（以下GPIOのピン番号設定は任意）
17. #define L_Motor_DIR 26 // ESB32のGPIO26を左モータドライバーA4988のDIRに設定
18. #define L_Motor_STEP 33 // ESB32のGPIO33を左モータドライバーA4988のSTEPに設定
19. #define R_Motor_DIR 32 // ESB32のGPIO32を右モータドライバーA4988のDIRに設定
20. #define R_Motor_STEP 25 // ESB32のGPIO25を右モータドライバーA4988のSTEPに設定
③ホイールの計測値を設定
注意：ホイールの直径と左右ホイールの間隔（トレッド）を正確に測定して設定します。
ここが正確でないと移動距離や旋回角度が狂ってしまいます。
26. float wheel_dia = 68.9; // ホイール（車輪）の直径を記します。
27. float wheel_base = 74.0; // 左右ホイールの間隔（トレッド：タイヤの中心点間の距離）を記します。
28. int steps_rev = 3200; // ステップモータ1回転のスッテプ数×マイクロスッテプ数（200X16）
④ラインセンサーの設定 // 各GPIOのピン番号設定は任意です。
35. const int lineFollowSensor0 = 18; // LO（左外側センサー）
36. const int lineFollowSensor1 = 5; // LI（左内側センサー）
37. const int lineFollowSensor2 = 17; // CT(中央センサー）
38. const int lineFollowSensor3 = 16; // RI(右内側センサー）
39. const int lineFollowSensor4 = 4; // RO（右外側センサー）
46. const int ledPin = 27; // LEDインジケーター
47. const int buttonPin = 23; // スタートボタン
２）maze_Function
オリジナルに準拠していますが、実演を見た家族にゴールした後に手で戻すの？との指摘に発奮！
通常ゴール後にスタートポイントへ手で戻しますが、自動で戻るプログラムを90行目からの
mazeEnd()関数に追加しています。
しかし、ゴールからスタートポイントへ左手の法則に従って戻るルートは、その後にスタートする最短ルートの
逆回りを走行します。結果、正解を先に示すことになり、手で戻した方が良かったのか？と悩むところです。
maze_FunctionのmazeSlove()関数では、sensor_Functionから得たmode信号により、分岐点の状況を
把握してモータを制御すると共に分岐点の状況をR・L・B・Sの4種類の分岐データとして記録し、
RLBSの法則に従って最短経路を計算しながらゴールまで走行します。
32行目からのcase RIGHT_TURN:と56行目からのcase LEFT_TURN:では、T字路の入り口で平行ラインに
対して車体が少し斜めに侵入すると左右に迷う現象が出ました。この対策として一度ストップして更に
5㎜前進させてもう一度センサーを読み取り、確認するようにプログラムを変えています。
131行目からのsimplifyPath()が短絡経路を計算する関数です。
注意：217行目からのvoid mazeTurn (char dir)；にも設定箇所があります。
ここでは、スタートポイントから最短絡ルートを走行する場合、各分岐点に達するとR・L・B・Sのターン信号が
出され、目的のゴールまで最短ルートを案内します。 ターン信号は1行目のmazeSolve(void)；で設定した
Turnright();、Turnleft();、Forward();、Straight();の値に準じます。
詳細は、Maze Solver Robotのチュートリアルをご覧下さい。
３）motor_Function
BasicStepperDriver及びSyncDriverを使ったステッピングモータコントロールに置き換えています。
main_Functionで定義したホイールの直径とホイールベースに基づいて移動距離や旋回角度が計算されます。
ステッピングモータの制御
前進のForward();は移動距離をミリメートル単位設定します。０で停止、例えば30㎜前進;の場合は
Forward(30)と記します。この値はホイールの中心線からフォトセンサーまでの距離にほぼ等しく
センサー基板の取り付け位置により異なります。
左右旋回のTurnright();、Turnright();は角度を記します。直角の場合は90を、Uターンは180と記します。
しかし、スピードが速いとストップを掛けた位置よりも慣性により進むので補正が必要です。
注意：各設定は実際にコースを走らせ、カットアンドトライで調整を行います。
４）sensor_Function
フォトセンサーの読み取り値を各コマンドに変換。（不要な部分を省略しています）
（LFS[]は行が長くなるのでlineFollowSensor[]を略しました）
2. LFS[0] = digitalRead(lineFollowSensor0); // Left out side sensor
3. LFS[1] = digitalRead(lineFollowSensor1); // Left in side sensor
4. LFS[2] = digitalRead(lineFollowSensor2); // Center sensor
5. LFS[3] = digitalRead(lineFollowSensor3); // Right in side sensor
6. LFS[4] = digitalRead(lineFollowSensor4); // Right out side sensor
ここで読み込んだセンサーの状態を、次のif文ににより５種類のコマンドに変換します。
①CONT_LINE：全てのセンサーが１＝黒の場合
②RIGHT_TURN：右外側のセンサーが１＝黒の場合
③LEFT_TURN：左外側のセンサーが１＝黒の場合
④NO_LINE：全てのセンサーが０＝白の場合
⑤FOLLOWING_LINE：中央センサー及び左右何れかの内側のセンサーが１＝黒の場合
尚、main_Functionの4行目から10行目にこれらの文字列を数値comndに変換を定義しています。
4. # define STOPPED 0
5. # define FOLLOWING_LINE 1
6. # define NO_LINE 2
7. # define CONT_LINＥ 3
8. # define POS_LINE 4
9. # define RIGHT_TURN 5
10. # define LEFT_TURN 6
走行速度について
その１でも述べましたが、このロボットは2020年8月に高精度お絵描きロボットとして製作したものを
少し改造して使っています。
自走しながら文字や図形を描くため、モータを1分間に７回転とゆっくりとした速度で動かせていました。
ここでは、迷路探索に適した速度として１分間に４０回転に設定していますが、６０回転位いまで可能です。
しかし、ステッピングモータが重いのでスピードを上げ過ぎると慣性が増し、細いOリングのタイヤでは、
ブレーキを掛けてもスリップして停止位置よりも余分に進むので設定が難しくなる弱点が有ります。
また、同様に90度旋回やUターンの180度旋回時も角度をオーバーし、その分の微設定が必要となります。
添付のプログラムコードを見れば、その辺りの設定差が解ると思います。
競技に使う場合は、タイヤにOリングを使わず、グリップが優れたタイヤを使うなどの対策が必要です。
おわりに
迷路ロボットはハードルの高い工作ですが、その分面白味が多いと思います。
説明不足が多々あると思いますが、迷路ロボットに興味のある方の参考になれば幸いです。
市販部品を多く使った3号車も完成してますが、プログラムの基本は同じなので説明を省略しました。
最後までご覧頂きまして有難うございました。
最後にもう一度動画をご覧下さい。
皆様の参考になれば幸いです。
by Paradise
スポンサーサイト
