RoboMasterS1を解析してROSで動かす！（みんなで動かそう編）

# ROSConJP 2019でLT発表しました
RoboMaster S1を改造したい皆さん、こんにちは。
だいぶ前から更新が滞っており申し訳ございません。
というのも、ROSConJPでLT発表してきたので、ネタバレ防止で更新していませんでした。
タイトルが変わったので気づかれたかもしれませんが、ROSからRoboMaster S1の操作に成功しました。

ということで発表資料はこちらにおいてあります。
https://www.slideshare.net/tatsuyaishikawa507/ros-con-jp-2019-lt-tatsuya-ishikawa
こんな感じでPS4のコントローラーからgeometry_msgs::Twistで操作できるようにしました。
ビデオは下をクリック↓
[![youtube](http://img.youtube.com/vi/u9NQQNjxe1s/0.jpg)](http://www.youtube.com/watch?v=u9NQQNjxe1s)
# Githubで公開
https://github.com/tatsuyai713/robomaster_s1_can_hack

とりあえずReadMeを英語で書くのがめんどくさいのでQiitaで日本語で書きます。
# 作ったもの
DJI RoboMaster S1 ROS Bridge Package
このパッケージにはROS-RoboMasterS1のブリッジノードとRoboMasterS1に搭載するNucleo-F767ZIのファームウェアのソースコードが含まれています。
## なんでNucleo
京都大学のT先生やいろいろな方面からmROSなどのマイコン側でROSノードをたてることを検討しましたが、諸般の事情（D○IのCANがクソすぎる）で、RTOS上ではCAN取りこぼしが発生したため、泣く泣くRTOSなしになりました。
で、なんでNucleoを選んだかというと、TwitterのアンケートでSTM32が人気だったのと、入手性、性能、一番重要なCANが2ChあってかつEthernetがあるということで、Nucleo-F767ZIを選びました。ただ今後micro-ROS([Github](https://github.com/micro-ROS/micro-ros-build/ "Github")）でROS2対応することも見越して、OLIMEX STM23-E407([Strawberry Linux](https://strawberry-linux.com/catalog/items?code=15075 "Strawberry Linux"))も手に入れています。

ただ、Nucleoのネジ穴間の距離がまさかのRoboMasterにはじめから開いている穴ピッチと同じという奇跡が起きて、簡単に機体に装着できます。（絶対DJI社内でもNucleo使っているでコレ

## で、こんなんできました
![robomasters1](https://user-images.githubusercontent.com/34103899/65610530-75044200-dfec-11e9-9dfc-d46c963ab85a.JPG)

## PS4コントローラーのキーマップ
 - 左ジョイスティック : Linear X と Linear Y
 - 右ジョイスティック : ジンバルのPitchとYaw
 - 右ジョイスティックのボタンのプッシュ : ブラスターを打つ
 - L2 : 押してる間だけ台車部分とジンバルが動きます
 
## まずは材料の準備
- DJI RoboMaster S1（高いけど頑張って買ってください
- UbuntuのPC（自分のは16.04LTS）STM32CubeIDEをインストール(これを参考にどうぞ[Booth](https://booth.pm/ja/items/1569100 "Booth"))
- Nucleo-F767ZI（MouserなりDigikeyなりで手配）
- ユニバーサル基板 (http://akizukidenshi.com/catalog/g/gP-09972/)
- CANトランシーバーTJA1050の乗ったBreakout基板 (https://www.amazon.co.jp/dp/B076HHVZM1) 
- ワイヤーハーネス
- 2.54mmピッチのコネクタとピン(https://www.amazon.co.jp/gp/product/B071RK5STD/)
- SONY PlayStation4(R) コントローラー

あと、無線で操作したい場合は以下のものも追加で買ってください。
- Ethernet-Wifiコンバーター (USBで電源供給できるので、WLI-UTX-AG300/Cが良い)
- DC-DCコンバータ (http://akizukidenshi.com/catalog/g/gK-09981/) （Nucleoからの5V電源では足りない）


## とりあえずインストール
今回は動かそう編なので、技術的なところにはあまり触れずにとりあえずやってみましょう。
PS4コントローラー用のドライバーds4drvのインストール
$ sudo pip install ds4drv

UbuntuのPCをWiFiアクセスポイント化するためにhostapdをインストール
$sudo apt install hostapd

設定ファイルを書くよ（内容は適宜使ってるPCのやつにしてちょ）
$sudo vi /etc/hostapd/hostapd.conf
`interface=wlp61s0`
`driver=nl80211`
`ssid=RoboMasterS1Host`
`hw_mode=g`
`channel=7`
`wpa=2`
`wpa_passphrase=robomasters1`
`wpa_key_mgmt=WPA-PSK`
`rsn_pairwise=CCMP`

GithubからCloneしてくる
$ git clone --recursive https://github.com/tatsuyai713/robomaster_s1_can_hack.git
このままではもちろん何もできないので、rosフォルダの中身を自分のROSのworkspaceに移動するなり、シンボリックリンク貼るなりしてください。

## 乗っ取り基板を作る
今回の方法では、インテリジェントコントローラーはそのまま生かした状態で、インテリジェントコントローラーとCANのネットワークの間にNucleoを入れます。つまりインテリジェントコントローラーのCAN指示を書き換えてしまいます。（なのでCANが2ch必要なのです）
まあ、基板作りはそれほど説明しなくても良いかと思うのですが、以下の点に注意してハンダ付けしてください。

 - RoboMaster S1のインテリジェントコントローラーのCANコネクタは上2ピンがCANのHighとLowで下がバッテリー電圧とGND（電源はちゃんとテスターで確かめてください・CANはどっちがHIGHか忘れました汗、後で書きます）
 - Nucleoの電源はバッテリー電圧をそのままVINに入れてもOKですし、DC-DCを介して5Vを供給してもよし
 - Wifiコンバーターの電源は500mAも食うので、DC-DCから供給すること
 - 通常CANは終端抵抗を適切に入れなければいけないが、今回使うBreakoutボードは終端抵抗が入っているのでそのまま繋いでOK
 - CANのBreakoutボードの電源は5V
 - CANのRX/TXは1ch:PD0/PD1 2ch:PB12/PB6となっているので、Nucleoの裏の文字に従ってBreakoutボードと接続
 - CANのH/Lの線はツイストペアにしましょう
 
## STM32CubeIDEでFirmwareをビルドして書き込む
さすがにこれまでは説明していられないので、これを買って勉強してください→[Booth](https://booth.pm/ja/items/1569100 "Booth")
ゆーくりっどさんありがとうございます。

# さて動かしてみましょう
## ネットワーク設定
ネットワーク設定ですが、Githubにあるaccess_point.shのとおり
PC：192.168.0.1
RoboMasterS1：192.168.0.2
となります。なので、WLI-UTX-AG300/Cの設定はそれに合わせて行ってください。
このスクリプトは標準のNetworkManagerを止めてIPアドレスを設定し、hostapdを立ち上げるようになっています。

まずは、PCをアクセスポイント化するわけですが、
$ sudo access_point.sh start
で動くはずですが、たまに起動に失敗するので。
$ sudo service hostapd status
で確認してFailしている場合は
$ sudo service hostapd start
で自分で立ち上げてください。

アクセスポイントを止めるときは
$ sudo access_point.sh stop
でOKです。

PCをアクセスポイント化できていればスマホなどでWifiのアクセスポイントが現れているはずです。
そうなれば、あとは説明書に従ってWLI-UTX-AG300/Cの設定をして、設定が完了したらRoboMaster S1の基板に接続しましょう。

## コントローラー設定
ここらへん読んで頑張ってください
http://cryborg.hatenablog.com/entry/2016/09/19/185501

PS4のコントローラーと接続して、コントローラーのLEDが青くなっていることを確認してください。

## RoboMaster S1の電源を入れて動かす！
まずはちゃんと組み立てて、スマホで動作できるところまではやっておいてください。
その後乗っ取り基板が正常に接続ができていれば、電源をONにすると通常の接続の場合と同様にモーションコントローラーのLEDが青点滅するはずです。もしうまく行かない場合はモーションコントローラーのLEDは赤点滅になります。その場合は接続を確認してください。
よくある間違いはCANのH/Lが逆だったり、NucleoとCANのトランシーバーの接続のRX/TXが逆だったりします。

電源をONにしたら、しばらくおいておいて、Wifiの通信が確立するまで待ちましょう。
その後、ROSのブリッジノードを以下の通り立ち上げます。
（みんなROS詳しいから大丈夫ですよね）
$ export ROS_MASTER_URI=http://192.168.0.1:11311
$ export ROS_IP=192.168.0.1
$ roscore&
$ roslaunch ros_robomaster_s1_bridge robomaster_s1_bridge_joy.launch

これであとはPS4のコントローラーで操作できます。
通信がたまに途切れたりするので注意してください。
きちんと通信できていると、コントローラーで操作するとそれに合わせて、NucleoのLANのコネクタの右側のLEDがチカチカと点滅します。これで通信できているか確かめられます。
LEDが点滅してい動かない場合は、RoboMasterS1側の接続不良を疑いましょう。

#次回
次回は詳細な技術的な内容で書きたいと思います。しばらくお待ちください。