fc2ブログ

迷路探索ロボット (Maze path search robot) その5(プログラム編)

MSR_02.jpg

参考プログラムについて
迷路ロボットを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を不使用

迷路探索ロボットのプログラム
注意:ハードウエアの仕様が変わるとプログラムを修正しないと使えません。
(長文になるので各ファンクションをクリックすると別ページにてプログラムコードが開きます)。
1)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; // スタートボタン

2)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のチュートリアルをご覧下さい。

3)motor_Function
BasicStepperDriver及びSyncDriverを使ったステッピングモータコントロールに置き換えています。
main_Functionで定義したホイールの直径とホイールベースに基づいて移動距離や旋回角度が計算されます。
ステッピングモータの制御
前進のForward();は移動距離をミリメートル単位設定します。0で停止、例えば30㎜前進;の場合は
Forward(30)と記します。この値はホイールの中心線からフォトセンサーまでの距離にほぼ等しく
センサー基板の取り付け位置により異なります。
左右旋回のTurnright();、Turnright();は角度を記します。直角の場合は90を、Uターンは180と記します。
しかし、スピードが速いとストップを掛けた位置よりも慣性により進むので補正が必要です。
注意:各設定は実際にコースを走らせ、カットアンドトライで調整を行います。

4)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文ににより5種類のコマンドに変換します。
①CONT_LINE:全てのセンサーが1=黒の場合
②RIGHT_TURN:右外側のセンサーが1=黒の場合
③LEFT_TURN:左外側のセンサーが1=黒の場合
④NO_LINE:全てのセンサーが0=白の場合
⑤FOLLOWING_LINE:中央センサー及び左右何れかの内側のセンサーが1=黒の場合
尚、main_Functionの4行目から10行目にこれらの文字列を数値comndに変換を定義しています。
4. # define STOPPED         0
5. # define FOLLOWING_LINE      1
6. # define NO_LINE        2
7. # define CONT_LINE       3
8. # define POS_LINE        4
9. # define RIGHT_TURN     5
10. # define LEFT_TURN      6

走行速度について
その1でも述べましたが、このロボットは2020年8月に高精度お絵描きロボットとして製作したものを
少し改造して使っています。
自走しながら文字や図形を描くため、モータを1分間に7回転とゆっくりとした速度で動かせていました。
ここでは、迷路探索に適した速度として1分間に40回転に設定していますが、60回転位いまで可能です。
しかし、ステッピングモータが重いのでスピードを上げ過ぎると慣性が増し、細いOリングのタイヤでは、
ブレーキを掛けてもスリップして停止位置よりも余分に進むので設定が難しくなる弱点が有ります。
また、同様に90度旋回やUターンの180度旋回時も角度をオーバーし、その分の微設定が必要となります。
添付のプログラムコードを見れば、その辺りの設定差が解ると思います。
競技に使う場合は、タイヤにOリングを使わず、グリップが優れたタイヤを使うなどの対策が必要です。

おわりに
迷路ロボットはハードルの高い工作ですが、その分面白味が多いと思います。
説明不足が多々あると思いますが、迷路ロボットに興味のある方の参考になれば幸いです。
市販部品を多く使った3号車も完成してますが、プログラムの基本は同じなので説明を省略しました。
最後までご覧頂きまして有難うございました。

最後にもう一度動画をご覧下さい。


皆様の参考になれば幸いです。
by Paradise
スポンサーサイト



テーマ : 電子工作
ジャンル : 趣味・実用

迷路探索ロボット (Maze path search robot) その4

プログラムについての予定でしたが、その前に3号機を作りました。
先に紹介の2号機には、自作のホイールやESP32マイコンの直付けなど工作に難易点があります。
各画像はクリックすると拡大し、鮮明に観れます。

超小型のマイコンXiao 2040を使用
3号機には市販のホイールや新しい超小型マイコンSeeed Stadio製のXiao 2040を使い、汎用性を重視しました。
しかし、何時も利用の秋葉原の通販サイトで購入したXiao 2040はAruduino環境でのデジタルIOが機能せず、
予想外に時間を費やしました。
Maze_2040_1.jpg

動作確認のためにRspberry pi Picoを仮付け
販売店には交換を依頼してますが、時間が掛かりそうなので同じ2040系マイコンを搭載したRspberry pi Pico
仮付けして動作の確認を行いました。
追記;5月3日に交換品と差し替えて完動を確認し、Rspberry pi Picoを撤去しました。
Maze_2040_2.jpg

完成後はこんな感じになります。
3号機の部品は、昨年10月に紹介の倒立2輪ロボットESP32 Evo2タイプロボットを作る その2
を改造して利用しました。電池はデジカメ用リチウムイオン電池を流用しています。
カメラ用電池以外の主要部品は全て秋月電子通商の通販で購入できます。
Maze_2040_3.jpg


次回はプログラムについて掲載予定です。

皆様の参考になれば幸いです。
by Paradise

テーマ : 電子工作
ジャンル : 趣味・実用

迷路探索ロボット (Maze path search robot) その3

MSR_03.jpg

動画が出来ましたのでご覧下さい。2つのコースを収録しました。
日本語の字幕を付けました。

次回はプログラムについて掲載予定です。

皆様の参考になれば幸いです。
by Paradise

テーマ : 電子工作
ジャンル : 趣味・実用

迷路探索ロボット (Maze path search robot) その2

迷路探索ロボットの紹介

迷路解決ロボットを検索すると多くのロボットには、DCギアモータやロータリーサーボモータを使った
作品などが多数紹介されいます。興味のある方は参考にして下さい。
今回の作品は、前回も少し触れましたが約2年前に作ったお絵描きロボットを改造して再利用しました。

主な構成部品
モータ:39㎜タイプのステッピングピングモータ
モータドライバー:A4988
マイコン:ESP32 Wroom32 (今回は無線通信を使わないのでArduino nanoなどでも可)
フォトリフレクタ:LBR-127HLD(秋月電子1個50円)
車体とホイール:アクリル板にて自作(ここでは正確さが求められます)

画像① 
ステッピングモータは、幅60㎜、長さ100㎜、厚さ2㎜のアクリル板の前方に取り付けています。
赤外線センサーにはLBR-127HLDを5個ユニバーサル基板に組み込み、モータの前側に取り付けています。
ホイールですが、左右の直径にバラツキが有ってはダメなのでアクリル板で自作しました。
ソフト編で説明予定ですが、BasicStepperDriverを使う関係により、ホイールの直径とトレッド(左右の間隔)が
正確でないと進む距離や回転角度が狂ってしまうからで、ホイールにブレが有ってもNGです。
組立時、車体の中心線に対して左右ホイールの位置が対称、及びIRセンサー基板の中央センサーの位置を
一致させることが大事です。
MSR_03.jpg

画像② 
制御基板は、お絵描きロボットの基板を流用ですが、モータの横から上側に取り付け方法を変更しました。
電池はデジタル一眼カメラ用の予備を使用。アクリル板をCNC加工して電池ホルダーを作りモータの
後ろ側に載せました。
MSR_04.jpg

画像③
裏側から見るとIRセンサー基板とボールキャスターが見えるだけです。
センサー基板は、ユニバーサル基板にセンサーモジュールをホール3個分を開けて等間隔に配置しています。
また、7ピンのL型ピンヘッダを取り付け、コネクターを介して制御基板と繋げています。(配線方法は回路図参照)
ボールキャスターにはタミヤ模型のNo.144を使いました。
MSR_05.jpg

回路図
電源に手持ちのデジタルカメラ用リチウムイオン電池を使いましたが、入手し易いニッケル水素充電池でも可能です。
ここでは、7.4VをMT3608昇圧型DC-DCコンバータを使い、12Vに昇圧してA4988モータドライバーへの印加及び
ROHM社製三端子型DC-DCコンバータBP5293-33にてESP32マイコン用の3.3Vを得ています。
今回は、WiFiやBluetoothの無線通信を使わないが、ESP32が載った使いまわしの基板をそのまま使用しました。
新たに作る場合は、Arduino nano等を使うと良いでしょう。
MSR_Circuit.gif

ステッピングモータを使った場合の長所と短所
長所:キビキビと動きが速く正確です。
短所:①モータが高価、②重い



動画を準備中
迷路探索の様子は文章で説明するよりも動画を見て頂くのがよく解ります。
現在、動画の準備をしていますので今しばらくお待ちください。

皆様の参考になれば幸いです。
by Paradise

テーマ : 電子工作
ジャンル : 趣味・実用

迷路探索ロボット (Maze path search robot) その1

迷路探索ロボットの概要
先に作ったライントレースカーを発展させ、迷路を走らせて最短経路を探索するロボットを作りました。
1台目はライントレースカーのフォトセンサーアレーを3個から5個に変更しただけですが、少し安定性に
欠けるので2台目を作りました。OSTRお絵描きロボットの過去記事を参照
OSTR_No3_12.jpg

2台目には2020年8月に製作したステッピングモータを使った高精度の「OSTRお絵描きロボット」を
改造して作りました。
MSR_02.jpg

迷路コース
先のライントレースカーと同様にスチロールパネルに電工用ビニールテープを貼り付けて迷路を作りました。

ロボットの動作
左下のStartポイントから出発して「左手の法則」に従って分岐点を辿り、右上のFinishポイントまで自走します。
通過した分岐点を記録して最短経路を自動計算します。
コース中のテープが途切れた場所ではUターンし、コースには余分な部分が無く全てのテープ上を辿ります。

次にロボットをStartポイントに戻して再スタートすると、今度は記録されたデータにより最短経路を通って
Finishポイントへ自走します。 
今はStartポイントまで手で戻していますが、Finishした後に自動で戻り再スタートするように改良の予定です。
MSR_01.jpg



次回からロボットの説明・動画・プログラムについて順次掲載予定です。


皆様の参考になれば幸いです。
by Paradise

テーマ : 電子工作
ジャンル : 趣味・実用

プロフィール

CNC_Paradise

Author:CNC_Paradise
木工・彫刻・機械工作・電子工作が
大好きで自作のCNCを使って工作を
楽しんでます。

最新記事
最新コメント
月別アーカイブ
カテゴリ
何でも工作室
検索フォーム
RSSリンクの表示
リンク
QRコード
QR