Rover製作09 Ardupirotで駆動系の設定をする

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| OS | Windows10 |
| MissionPlanner | v1.3.80 |

| 項目 | 値 |
|:-:|:-:|
| Board| Pixhawk(v2.4.8) |
| Ardupilot | Rover V4.2.3 |

# 概要

とうとう移動ロボットの要の駆動系部品の組み立て、設定を行います。Pololuのエンコーダー付きギアモーターを使い、WheelOdometryが使えるようなセットにします。モータードライバとしてRoboClawを使います。今回はモーターをプロポ経由でコントロールすることを目標にします。

![09_controll.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/e01876c0-20c1-5c9b-9b65-5a03d922438f.jpeg)

# 予備知識

## バッテリーモニター

今回使うPDBにはバッテリーモニターが組み込まれていて、バッテリー電圧と使用電流を計測することが出来ます。
電流系はDroneでの数十Aの大電流を想定したもののようで、Roverで使う数Aスケールだと誤差は大きめです。
また、残量も起動時は100%と表示して、消費電力を積算して表示しているようであまりあてにはなりません。

## バッテリーフェイルセーフ
Ardupilotではバッテリー残量が低下したときにアクションを行う機能があります。ロボットを動かしているとバッテリー定電圧になったことを見落とすこともあるので設定しましょう。バッテリー残量の閾値とアクションの内容をパラメーターで設定できます。

## Onboard Displayの設定(I2C)
モードの状態やArmの状態を表示してくれます。MissionPlannerをつなげば大体のArdupilotの情報が見えるのですが、今後組み込んだ時にArmできないときなどに理由を表示してくれるので便利です。対応しているディスプレイが2種類ありますが今回は「SSD1306」を使います。これはI2C接続の物ですが、同じSSD1306でもピン配列が違うものがいくつかあるので注意です。
ardupilotの機能で、FPVゴーグル用の映像伝送に文字をオーバーレイするOSD(On Screen Diaplay)があるのですが、これとは別機能です。

![DSC_1243.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/869fb3ef-c461-4648-7da5-c2addb347e5f.jpeg)

## SafetySwitchについて
このスイッチが「safe」である間はモーターが回らないようになっています。初期設定では起動時は「safe」でボタンを長押しすると切り替わります。パラーメーターを変更すると起動時「unsafe」に変更できます。

|状態|LED|説明|
|:--|:--|:--|
|safe|flashing|モーターが回らない状態|
|unsafe|solid|モーターが回る状態|

## ARMとprearm check
Ardupilotは起動時は「DISARMED」状態でARM操作をすることで「ARMED」状態になります。
ARM操作をするときに「prearm check」が行われて、これがNGが出ると「ARMED」状態にはなりません。
デフォルトでは様々な項目をチェックしていますが、今回の使い方では、GPS受信できていない等の確定でNG項目があります。チェック通過するために「prearm check」で検査する項目を編集します。

## Ardupilotのモードについて

|モード名|説明|
|:--|:--|
|Hold|モーターを動かさないモードです|
|Manual|スティック軸の値をそのまま出力するモードです|
|Acro|自己位置のフィードバックをしながら出力をするモードです|
|Guided|自律機能が使えるモードです。CompanionComputerからの指令値に従う動作もできます。|

# 事前条件

必用なハードウェア

* WindowsPC
    * MissionPlanner導入済み
* Pixhawk(ardupilot roverインストール済み)
    * MissionPlannerと接続可能なこと
* バッテリー＆モーター一式
    * 過去回で製作済みの物
* Futaba送信機＆受信機
* I2C接続モニター（SSD1306）
* ブザー
* セーフティースイッチ
* サーボケーブルx2

# 手順

## PDBとPixhawkの接続

* PDBの5V電源とPixhawkの「Power」コネクタを接続する。

## バッテリーモニターの設定

* WindowsPCでMissionPlannerを起動して、Ardupilotと接続する
* 「初期設定」->「オプションハードウェア」->「バッテリーモニター」を選択する。
* 以下のように設定する
    * モニター Analog Voltage and Current
    * センサ 4:3DR Power Modul
    * APM ver 4:The Cube or Pixhawk

![bat1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/5fc94b10-4e11-6011-d011-a0b4203d1065.png)

* Pixhawkに電源を再投入する。
* 「フライト・データ」で表示されるウィンドウで以下のように電圧・電流・残量が表示されることを確認する。

![bat2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/5f7456ce-228d-9a17-0878-3d7c7d443820.png)

## バッテリーフェイルセーフの設定

* WindowsPCでMissionPlannerを起動して、Ardupilotと接続する
* 「設定/調整」->「パラメーターツリー」を選択した画面で、以下のパラメーター設定をする。

| 項目 | 値 | 説明 |
|:-:|:-:|:-:|
| BATT_LOW_VOLT | 15 | 15Vを下回ったら発動 |
| BATT_FS_LOW_ACT | 2 | HOLDモードに遷移する |

* 「パラメーター書き込み」を押す。

## 周辺機器の接続

* ssd1306のI2CコネクタをI2Cエクステンダー経由でPixhawkのI2Cコネクタに差し込みます。
* ブザーをPixhawkの「BUZZER」コネクタに差し込みます。
* セーフティースイッチをPixhawkの「SWITCH」コネクタに差し込みます。

## OBDの設定

* WindowsPCでMissionPlannerを起動して、Ardupilotと接続する
* 「設定/調整」->「パラメーターツリー」を選択した画面で、以下のパラメーター設定をする。

|パラメーター名|値|説明|
|:--|:--|:--|
|NTF_DISPLAY_TYPE|1|ssd1306を使用する|

* 「パラメーター書き込み」を押す。
* Pixhawkの電源再投入後に、OBDに情報が表示されることを確認

## RoboclawとPixhawkの接続

ArdupilotのサーボレールとRoboclawの接続をします。

|Pixhawk(サーボレール)||Roboclaw|
|:--|:--|:--|
|MAIN1||S1|
|MAIN3||S2|

## ServoOutputの接続

* PixhawmのサーボレイルとRoboclawをサーボケーブルで以下のように接続する。

| Pixhawk | | Roboclaw |
|:-:|:-:|:-:|
| Main1 |   | S1 |
| Main3 |   | S2 |

## 駆動系パラメーターの設定
共通の手順としてMissionPlannerでardupilotを接続します。Roverの設定をしていきます。

|パラメーター名|値|説明|
|:--|:--|:--|
|FRAME_CLASS|1(default)|Rover|
|SERVO1_FUNCTION|73|左モーター出力|
|SERVO3_FUNCTION |74|右モーター出力|
|MOT_SLEWRATE|300|モーター出力の変化率の最大値の設定[%/s]|
|BRD_SAFETYENABLE|0|safety switch が起動時にunsafeになる|
|ARMING_CHECK|310|Barometer、Compass、INS、Parameter、Battery Levelのみチェック|

## モーターの駆動動作確認

* safety swtichが「unsafe」であることを確認する。
* 「初期設定」->「オプションハードウェア」->「モーターテスト」を選択する。
* モーターが回転しても安全であることを確認する。
* 「Test motor C」を押す。
    * この時左モーターが進行方向に向けてゆっくり(30deg/s程度)で2秒ほど回ることを確認
* 「Test motor D」を押す。
    * この時右モーターが進行方向に向けて同様に回ることを確認

![motor_test.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/5526224a-428d-d64f-e426-f3d59d178903.png)

## プロポ操作でのモーターの動作確認

* safety swtichが「unsafe」であることを確認する。
* 左肩のスイッチを奥に倒し、フライトモードを「HOLD」にする。
* 右スティック左右軸を右いっぱいに数秒間倒し続けるARM操作をする。
    * 正常にARMが完了するとブザー音が鳴る。
* 左肩のスイッチを真中に倒し、フライトモードを「MANUAL」にする。
* ボリューム軸で前進後退、右スティック軸左右で旋回します。

![motor_test2.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/031d4fe2-e1a0-8b11-a33c-16a22aed3409.gif)
※上記の動画では軸の設定が違うため右スティック上下で前進後進方向の動きをしている。

* Roboclaw単体の時とは違い、各軸の情報がMixされた出力となっていることを確認する。

## WheelEncoderの設定

* WindowsPCでMissionPlannerを起動して、Ardupilotと接続する
* 「設定/調整」->「パラメーターツリー」を選択した画面で、以下のパラメーター設定をする。

ピンの設定
|パラメーター|値|説明|
|:--|:--|:--|
|servo11_function|-1|AUX3をGPIOとして使う|
|servo12_function|-1|AUX4をGPIOとして使う|
|servo13_function|-1|AUX5をGPIOとして使う|
|servo14_function|-1|AUX6をGPIOとして使う|

エンコーダー1(左側)の設定
|パラメーター|値|説明|
|:--|:--|:--|
|WENC_TYPE|1|WheelEncoder有効|
|WENC_CPR|-4480|1回転あたりのエンコーダーのパルス数(負値は回転方向逆を示す)|
|WENC_PINA|55|エンコーダー1のA相はAUX6|
|WENC_PINB|54|エンコーダー1のB相はAUX5|
|WENC_POS_Y|-0.17|モーター1のY位置[m]|
|WENC_RADIUS|0.072|モーター1のタイヤの半径[m]|

エンコーダー2(右側)の設定
|パラメーター|値|説明|
|:--|:--|:--|
|WENC2_TYPE|1|WheelEncoder有効|
|WENC2_CPR|4480|1回転あたりのエンコーダーのパルス数|
|WENC2_PINA|53(default)|エンコーダー2のA相はAUX4|
|WENC2_PINB|52(default)|エンコーダー2のB相はAUX3|
|WENC2_POS_Y|0.17|モーター1のY位置[m]|
|WENC2_RADIUS|0.072|モーター1のタイヤの半径[m]|

* Pixhawkに電源を再投入する。

## エンコーダーの動作確認

* 「エンコーダー接続ケーブル(Pixhawk)」で上部配線基板とPixhawkを接続する。
* WindowsPCでMissionPlannerを起動して、Ardupilotと接続する
* 「初期設定」->「Advanced」を選択、中央画面の中から「MAVLink Inspector」を選択する。
* ツリーを開いて「WHEEL_DISTANCE」の値を確認する。
* 駆動輪を回して、各軸が進行する方向に1[m]分進むと、1.0数値が増えることを確認する。
    * 最初の要素が左エンコーダー、2つ目が右エンコーダーに対応する。

![WHEEL_DISTANCE.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/01833a8a-712e-0bd9-6510-e09146bd07be.png)

# 参考
[Ardupilot Rover モーター設定](https://ardupilot.org/rover/docs/rover-motor-and-servo-configuration.html)
[Ardupilot Rover Failsafe](https://ardupilot.org/rover/docs/rover-failsafes.html)
[Ardupilot WheelEncoder](https://ardupilot.org/rover/docs/wheel-encoder.html)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)
[ROS2講座の目次へのリンク](https://qiita.com/srs/items/5f0d765343ca78f524ed)