Toioで遊んでみる (2)

前回はToioのキューブに対する基本的な操作まで触れたので、今回はそれらを組み合わせて「キューブを好きな座標に移動する」といったナビゲーション処理について書いてみる。

ナビゲーション処理

このへん: https://github.com/tomoto/tomotoio/blob/master/tomotoio/navigator.py

キューブの底面にはカメラが付いており、マットやシールに印刷された特殊パターンを読み取って、「今マット上のどの座標にどの角度でいる」とか「今どのシールの上に乗っている」といった情報(仕様書の用語だとToio ID)がわかる仕組みになっている。こういった位置情報に基づき、キューブが目的の位置なり姿勢なりに到達するようモーターにフィードバック制御をかけるのがこのナビゲーション処理である。

このナビゲーション処理は、位置情報のNotificationストリームに駆動される形で実装されている。つまり、Notificationのコールバックの中で次のモーターの状態を決定して指示を出す。そのため、Notificationストリームがスムーズに流れない環境であれば意図しない動作をする可能性がある。また、タイミングの変化を相殺する仕組みもあまり入れていないため、ストリームの流れる速度が変われば(例えば20イベント/秒で流れる環境と5イベント/秒で流れる環境とでは)加減速カーブなどが変わってしまう可能性がある。結局のところ、自分のRaspberry Pi 4に合わせてロジックやパラメタを調整しているので、環境が変わればそれなりの対応が必要かもしれないことに注意して欲しい。

指定された方向への回転(rotate)

このへん: https://github.com/tomoto/tomotoio/blob/master/tomotoio/navigator.py#L74

rotate.py でデモしている動きである。ナビゲーションの中では最も簡単。なるべく迅速に方向が決まるよう、現在の角速度、目標角度までの残り角度などを用いて、スムーズにモーターの速度を上げ下げする工夫をしているくらい。目標精度±3度くらいシャキッと余裕で決まる。

指定された座標への移動(move)

このへん: https://github.com/tomoto/tomotoio/blob/master/tomotoio/navigator.py#L113

symmetric.py でデモしている動きである。一見簡単そうだが見た目ほど自明ではなく、様々なアプローチが考えられる類のもの。変にカクカクした動きや無駄な大回りを避けるため、まず目標座標が自分のおよそ前方(デフォルトで±30度以内)になければ前節のrotateナビゲーションを行い、およそ前方にあればそこに至る円弧の曲率を計算して左右のモーターの速度を設定する、という方式にした。目標地点でちゃんと止まれるようにスムーズな速度の上げ下げを行うのは前節と同じ。将来的には経由点を指定しできるようにして、それらを滑らかに踏んでいくような処理にしたい。

指定された円上の周回運動(circle)

このへん: https://github.com/tomoto/tomotoio/blob/master/tomotoio/navigator.py#L177

circle.py でデモしている動きである。円の中心座標と半径を指定すると、その円上をキューブが周回運動する。実装的には、目標円上の自分に一番近い点から接線方向ちょっと先の座標を目標として、前節のmoveナビゲーションをかけるというふうになっている(したがい実際の移動軌跡は目標円より若干外側になると考えられる)。目標座標は常に接線方向ちょっと先に更新されるので、「人参をぶらさげた馬」のように決して止まることはない。回転方向はデフォルトで時計回りで、マットからはみ出しそうになると回転方向を反転する。

目標パラメタの動的な変更

上記3種類のナビゲーションすべて、目標角度・目標座標・目標円のパラメタを動的に変えることができ(updateTarget())、キューブはそれに追随して動く。

モーターの時限制御

モーターを回すときには1秒ちょっとの時間制限をかけておくのがよい。理論的には無限時間でもいいのだが、マットから落ちたりしてNotificationストリームが止まった時にモーターが延々と回り続けるのは具合が悪い。1秒に何度もモーターの状態設定が走るので、

マット情報

位置情報から得られる座標は仕様書のここに書かれているようにマットによって値の範囲が異なる。(ちなみに工作生物ゲズンロイドのマットは認識してくれない。何らかの隠しオプションを設定する必要があるのだろう。)

Navigatorには自分がどのマット上にいるかの情報が乗っており、マット上の相対位置取得やはみ出し防止処理を書けるようになっているが、現時点ではトイオ・コレクションマットの表側に決め打ちしてある。(See https://github.com/tomoto/tomotoio/blob/master/tomotoio/navigator.py#L59)

一人サッカー

最初に紹介した一人サッカーについて軽く紹介してみよう。
コードはこれ: https://github.com/tomoto/tomotoio/blob/master/examples/soccer.py

ボールの動き

このへん: https://github.com/tomoto/tomotoio/blob/master/examples/soccer.py#L21

ボールは独立・自律的である。つまり、プレイヤーなど周囲の環境に依存することなく、ちょんと押されたらその方向にしばらく動いて止まるだけである。これはカスタムナビゲーションロジックとして実装し、Navigator.setCommand()でボールのNavigatorに設定してしまうのが楽である。こうすればNotificationスレッドの側で勝手に処理されるので、あとは忘れておいてよい。

「ちょんと押された」の判定はなかなか難しい。感度は高い方がよく、しかしノイズを拾って勝手に動き出さない程度にしたいのだが、結局ぎこちない反応になってしまった。

プレイヤーの動き

このへん: https://github.com/tomoto/tomotoio/blob/master/examples/soccer.py#L85

ボールとは異なり、メインループの方で実装している。ナビゲーションとして実装することもできるが、まあ好みである。

• 基本的にはボールに向かってmoveナビゲーションで突っ込む。
• ボールを蹴ったらちょっと後ろに下がってボールが止まるのを待つ。
• そればかりやっているとボールをマット外に蹴り出してしまうので、ボールがマットの端に近い場合はcircleナビゲーションを使って外側に回り込む。
• moveしているはずなのに同じ位置にとどまっていることが検出されたら、何かにstuckしていると判断し、ちょっと後ろに下がって仕切り直す。
• 状態に応じていろいろ音を鳴らす(setMusic())。

もう一台キューブがあれば、互いに相手のゴールにボールを蹴り込む自律ゲームが作れるんだけどなー。