toioでメカナムホイール制御を楽しむ

#サマリ
今回の記事では、toio™️のキューブを使ってメカナムホイール制御を楽しむ方法を提供しています。具体的には、下記の動画のような制御が(物さえ揃えば)すぐに楽しめるようになります。
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr"><a href="https://twitter.com/hashtag/toio?src=hash&amp;ref_src=twsrc%5Etfw">#toio</a> のVisual Programmingでメカナムホイールを完全に理解した。<br>ついにtoioのキューブは全方位移動を手に入れましたww<br><br>先日の機構モジュールの応用編となりますが、詳細は長くなるので、以下のQiita記事にて。<a href="https://t.co/3YrRGB4cYp">https://t.co/3YrRGB4cYp</a> <a href="https://t.co/nSYxmhL0Ua">pic.twitter.com/nSYxmhL0Ua</a></p>&mdash; Tetsunori NAKAYAMA | 中山 哲法 (@tetunori_lego) <a href="https://twitter.com/tetunori_lego/status/1210191833608769536?ref_src=twsrc%5Etfw">December 26, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>
YouTube -> https://youtu.be/c4uEohRcmfE

一家に一台、手乗り全方位移動ロボットを。
![IMG_0533.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/542654/3c6b5b49-fc0d-56c9-d1ac-ab749b1a6606.jpeg)

#きっかけ
最近、toioのVisual Programmingについて、Windows対応/2台制御対応がそれぞれ提供された([こちらの記事](https://qiita.com/eqot/items/84983604efdac693b5b8)も参照)ので、これらを使って何かできないかなーとぼんやり考えていましたが、[先日の機構モジュール](https://qiita.com/tetunori_lego/items/9d3168bccfe0bd685523)が2個あれば4動力使えますし、もともとキューブを制御するにあたり、**横移動できないこと**にストレスを感じていたので、それを解決するソリューションを作れないか？が開発の発端になっています。

#検討方針
とにかく、簡単に準備して作ってバラせること。この方針はブラしません。
そのうえで、ロボットの全方位移動手段について調べてみると、大抵はオムニホイール、メカナムホイールが出てきます。今回は入手容易性も考慮し、スイッチサイエンスさんで販売していてかつ在庫も豊富な**[プラスチックメカナムホイール](https://www.switch-science.com/catalog/5939/)**を選択することにしました。動力も４つでちょうどいいし！

#準備
##用意するもの
![IMG_0509.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/542654/8b17e0f2-4bff-e8c3-c676-3d569ba3b7e2.jpeg)

 - キューブとLEGOブロックを含む[機構モジュール](https://qiita.com/tetunori_lego/items/9d3168bccfe0bd685523)：2個
 - [プラスチックメカナムホイール](https://www.switch-science.com/catalog/5939/)：左右2個ずつ
    - スイッチサイエンス製品。直径64mm、幅30mm
 - LEGO 2*14 プレート( [91988](https://www.bricklink.com/v2/catalog/catalogitem.page?P=91988) )：1個
 - LEGO テクニック 軸コネクター( [6538](https://www.bricklink.com/v2/catalog/catalogitem.page?P=6538) )：4個
 - LEGO テクニック スタッド付き軸( [6538](https://www.bricklink.com/v2/catalog/catalogitem.page?P=6587) )：4個
 - (WANT) LEGO テクニック ブッシュ 1/2( [4265c](https://www.bricklink.com/v2/catalog/catalogitem.page?P=4265c) )：4個
  -  後述しますが、あると便利。
 - [toio™ Visual Programming( **dev版** )](https://toio.github.io/toio-visual-programming/dev/)が使用できるPC( Windows10/Mac )：1台
 - [こちら](https://github.com/tetunori/MechanumWheelControlVisProg/archive/1.0.0.zip)からVisual Programming用の.sb3ファイル(zipを展開すると"MechanumWheelControlVisProg.sb3"が入っています)をダウンロード。

##HWの組み立て方
1. [こちらの記事](https://qiita.com/tetunori_lego/items/9d3168bccfe0bd685523)を参考に機構モジュールを2つ作成してください。![IMG_0510.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/542654/5480b3a2-370d-8372-c83a-0872d2c4b5c1.jpeg)
3. 機構モジュールのホイールに軸コネクター4個をぶっ刺します。<img width="2016" alt="IMG_05132.JPG" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/542654/63e9c23f-70e6-1764-90f1-4a7c272719b0.jpeg">
4. 写真の様に、各メカナムホイールに外側からスタッド付き軸を刺し、軸コネクターと接続します。これを4つ全てのホイールについて実施します。
![TKGK4294.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/542654/589ea0e2-12d3-7b2e-6133-7967af1dfd5d.jpeg)
この時、スタッド付き軸に対して、内側からブッシュをつけてあげるとタイヤが軸上をスライドしなくなるので、かなりロバストになります。
![NZET2709.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/542654/970e208b-4039-163a-5ff4-61315a320721.jpeg)
※ なお、**各ホイールには向きがありますので前後左右間違えないように取り付けてください**。覚え方として、タイヤの向きに沿って直線を引いた時、その線が中央付近を通るように配置すれば良い事に気が付きました。
![IMG_0525x.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/542654/e29111ed-f5a6-8860-9fd3-6ecd5aa985cc.jpeg)

6. 最後に、キューブの前が揃うように2*14 プレートの端と端にキューブをくっつけます。これで完成です。
<img width="910" alt="completed.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/542654/44fbbbfc-4bce-0f9d-3ea5-a2ab725f6f79.png">


#動かしてみよう
##メカナムホイールの制御原理
難しい構造についてのテキストは、[ここ](http://ysserve.wakasato.jp/Lecture/Fujisawa1/node7.html)や[ここ](https://en.wikipedia.org/wiki/Mecanum_wheel)を見てほしいですが、ソフトウェア制御にまつわる内容は["How a Mecanum Drive Works"](https://seamonsters-2605.github.io/archive/mecanum/)が易しく、詳しかったです。諸々をサマると下記の通り。

###全方位移動の制御
- 左上・右下、左下・右上のホイールはそれぞれ全く同じ動作をします。
- ホイール制御式は、xを目標の進行方向(radian)、yをホイール制御の単位量とした時、下記で表現できます。なお、実際に制御するときはyの値にスピードを掛け算してください。
$$ 
左上・右下 ⇒ y = \sin(x+π/4) \\\\
右上・左下 ⇒ y = \sin(x-π/4)
$$

###回転制御
- キューブ間の中点、Headキューブ/Tailキューブ、左右後輪などを中心とした回転動作が可能。詳細は以降のアプリのUIや実動作を見ていただきたい。

##SWアプリケーション
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/542654/7d368006-92c3-bf49-9400-8eeea503a90c.png)

###起動方法
1. [toio™Visual Programmingの開発者版](https://toio.github.io/toio-visual-programming/dev/)で、上記ダウンロードしたファイル"MechanumWheelControlVisProg.sb3"を開きます。
2. キューブを2台接続します。この時、**qキー**を押すとHeadキューブのライトを点灯できますので、Head/Tail逆に接続してしまった場合は、**1/2キー**を使ってHead/Tailを切り替えてください。
3. 緑の旗を押して準備完了です。

###操作方法
|カテゴリ  |操作方法  | UI表示例  |
|---|---|---|
|全方位移動|↑/↓/←/→<BR>の組合せ<BR>8方向|<img width="490" alt="cursor.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/542654/9f52ff9d-fd35-a8e6-867e-4c80065c21cc.png">|
|真ん中(赤い点)<BR>を中心に回転 |r + ←/→|<img width="488" alt="rotation.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/542654/fd2cb8e4-75c3-a0a9-361e-8501521b1261.png">|
|Head/Tail(赤い点)<BR>を中心に回転 |h/t + ←/→|<img width="489" alt="headtail.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/542654/5b3b8f8b-b41c-68e2-7333-9521713a7f61.png">|
|左右車輪(赤い点)<BR>を中心に回転 |d/f + ←/→|<img width="488" alt="fd.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/542654/5f3405bb-8b04-e442-e301-6d0c49c2ede6.png">|
|Headキューブの確認 | qキー |Headキューブのランプを白色点灯|
|Head/Tailキューブの交換 | 1/2キー |1: キューブ1をHeadに。<BR>2: キューブ2をHeadに。|
|走行スピード調整| スライダー | <img width="165" alt="Speedslider.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/542654/b40edcd0-a963-4b6e-21a2-095fdd9591f5.png"><BR>走行するスピードの動的設定。<BR>デフォルトは40。 |

###実際に動かしている様子
- 全方位移動などの様子(再掲)
    - Twitter -> https://twitter.com/tetunori_lego/status/1210191833608769536
    - YouTube -> https://youtu.be/c4uEohRcmfE
- 回転動作をより詳細に
    - Twitter -> https://twitter.com/tetunori_lego/status/1210192042141184002
    - YouTube -> https://youtu.be/LZZqrjRLbag

###SWのポイント
####キューブのモーター制御は逆回転になるよ
機構モジュールはキューブのタイヤとレゴのタイヤの摩擦で成り立っているので、SWで指定した回転方向とメカナムホイールの回転方向は真逆になります。よって、速さは-1倍します。<BR><img width="476" alt="cube_speed_reverse.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/542654/2de546ac-434e-69d7-6433-d31dfe3ab466.png">
####全方位移動の実装
上述の全方位移動の数式はこのような感じにシンプルにまとめられます。なお、Visual Programmingのsin関数はradianではなくdegreeなので、sinに食わせる前に180/πをかけます(ちょっとハマった)。<BR><img width="493" alt="move_in_direction.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/542654/e485c5f4-ff13-272b-fedd-6d4bfa10cb32.png">
####組み合わせを含むキー入力の取り方
キー入力は1, 2, 4, 8...を各キーにアサインしてその合計値を最終的なコマンドとして読み取ってコマンド判定しています。<BR><img width="522" alt="keyinput.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/542654/ebaa2040-edaf-f7c9-4908-0709223a24ff.png">
####矢印の下の透明座布団表示・非表示
座布団だけ半透明にしたかったので、別スプライトへメッセージを送りそちらで半透明座布団を表示・非表示を行っています。<BR><img width="370" alt="message_send_receive.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/542654/af328dc2-401f-d93c-b51d-719544f75e33.png">

#所感と考察
- メカナムホイールを考えた人、天才だな。
- 全方位な動作は通常の人生では出会わないことが多いので、妻子どもの驚いた反応が新鮮。子どもに至っては「全部、車これにすればいいのに」のコメントあり。いろいろあるんだよ。
- HWは簡単にバラして再利用可能であり当初の目的は大変良く達成されたが、少し強く衝撃が加わると車軸が抜けてしまうこともあり、再利用性とロバストネスのトレードオフは悩ましかった。
- Visual Programmingは最初から制御ブロックが提供されているので、無線回りの実装に時間をかける必要がなく、かなり初期段階からすぐに原理確認が出来たのは良かった。一方で凝ったことはやりにくいので、一通り原理確認できたら、JavaScriptなどに移行することをお薦めします。
- また私の動作環境(vaio VJP132 / Windows10 Home)では、勝手にキューブの接続が切れることが多々あるが、根気良く再接続しているといつか安定しますww。
- 今回のロジックを使えば、全方位移動は8方向だけではなく、より細かいアナログ的な値を取っても動作すると思われるので、次回はDualShock4のアナログスティックで操作できるようにしたい。
- toioならではの制御として、位置座標による自動全方位移動制御も確認してみたい。