ししかわのマウス研修 マウス自作研修

制御ループの実装 – ししかわのマウス研修 Part.47

ししかわのマウス研修

ししかわです。

社員研修の一環で、マイクロマウスを自作して大会に出場しました

記事一覧

全日本大会2020は終わりましたが、ブログはもうちょっとだけ続きます。

今回はセンサやコントローラを統括するマウスクラス(図中赤枠)と制御ループの実装です。

Mouseクラスの実装

Mouseクラスは走行指令を受け付けて、制御ループを通じてマウスを実際に走らせるためのクラスです。

基本的には今まで紹介してきたドライバやコントローラのメソッドを順番に呼び出すだけで、それほど複雑なことはしていません。

マウスの走行時の動作

マウスに走行指令を与えることで走ります。走行指令は「180mm直進」「右に半径90mmで旋回」など、距離と走り方の情報を持ちます。探索走行時は(既知区間加速を除いて)1マスずつ走行指令を与えますが、最短走行の際はスタートからゴールまでの経路をすべて算出してからまとめて走行を与えます。

マウスクラスで複数の走行指令をまとめて実行するために、キューを使って走行指令を格納し、順番に取り出しながら走れるようにします。

走行指令(Motion)の実装は次のようになっています。走行の種類と目標座標のほか、開始時・終了時の速度が与えられるようになっています。

制御ループの実装

2つの制御ループをタイマー割り込み使って実装しました。

制御ループ全体を1kHzで駆動したいですが、このとき問題になるのが赤外線センサです。壁との距離を測るために赤外線LEDをOn/Offしてフォトトランジスタの受光量の差分を用いますが、同時に全てのLEDを切り替えると電流量のピーク値が大きくなり、十分な光量を得られない可能性があります。また、M5Mouseで使っている受光素子(フォトトランジスタ)は指向性が高いとはいえ、LEDを同時に光らせると互いに干渉する可能性もゼロではありません。

そこで赤外線センサのみ別のループで制御し、1回のループでは1つのLEDだけ操作することで、電流量のピークを抑えて干渉を無くします。今回は制御ループの4倍の周波数を持つタイマーで駆動します、こうすると1kHzの制御ループが1周するあいだに、ちょうど4つの赤外線センサの更新ができます。

メインの制御ループ

制御ループ1回分はm5mouse_update関数で行います。次のような流れです。

  • 壁の情報を更新する
  • マウスの現在の速度を更新する
    • 左右の車輪のエンコーダから回転量を取得し、車輪の速度に変換する
    • ジャイロセンサから角速度を取得する

  • マウスの自己位置(オドメトリ)を更新する
  • 次の目標速度を求める
    • 走行指令が終了していてキューが空でなければ、走行指令をキューから取り出す

  • 目標速度に近づくようモータを駆動する
    • PID制御を使う
      • PIDゲインはマウスを実際に走らせながら試行錯誤して決めました。

 


 

ここまででファームウェアの説明を一通りしてきました。

ファームウェアのソースコードや回路図、PCBのレイアウトなど、設計情報一式をGitHubのリポジトリで公開しています。

meganetaaan/M5Mouse
A micro mouse module made for M5Stack. Contribute to meganetaaan/M5Mouse development by creating an account on GitHub.

M5Mouse特有のパラメータ等もあるためそのまま使うのは難しいですが、ぜひ参考にしてみてください。

タイトルとURLをコピーしました