ROSとpigpioによるモータ制御

[ROS2関係トップページへ](https://qiita.com/NeK/items/7ac0f4ec10d51dbca084)

Raspberry Pi 3B+でモータの回転方向・回転速度を制御するプログラムの作成する．
ここではモータドライバとしてTA7291Pを使用する．本当はPWMを使うには向いていなさそう．．．またPWMによるモータの回転速度を制御する方法は，TA7291PのIN1, IN2にPWMを使用する方法(メジャー)とVrefにPWMを使用する方法がある．

- INにPWMを使う場合
    - メリット
        - Vrefに加える電圧(実質モータにかける電圧)を自由に選べる
    - デメリット
        - CW(時計回り)ではIN1にPWM信号を，CCW(反時計回り)ではIN2にPWM信号を与えなければならない．よって一つのモータに対して二つのPWM信号生成器が必要
- VrefにPWMを使う場合
    - メリットとデメリットが逆

# 準備
[ROS2でpigpioの利用 -準備・情報-](https://qiita.com/NeK/items/da429f8c001476ec4544)

## 回路の準備
とりあえず適当に．OUT1, OUT2をモータに接続し，Vref, IN1, IN2をraspberry piのgpio 18, 23, 24に接続する．


![motor_control.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/284261/a23bbac4-ab23-7b71-57f4-35d53bfdd9e6.png)

# ROS2プログラミング
作成物:[github](https://github.com/nek009/pi_motor_control)

ワークスペース名

- pi_motor_control

ワークスペース以下のパッケージ名

- pi_motor_control_msgs
    - このプロジェクトで使用するメッセージ用パッケージ
- pi_motor_control_nodes
    - モータを制御するserviceを提供するnode用パッケージ
- pi_motor_control_tests
    - nodeを実行するためのターゲット用パッケージ

## 概要
PiMotorControlNode::handleService_がモータ制御する部分．
gpioWriteでIN1とIN2を操作し，CW, CCWの制御を行っている．
注意点としては，回転方向を変える場合，一旦モータをSTOPにして100μ秒待ってから動かすこととある([TA7291Pのデータシート](https://www.marutsu.co.jp/contents/shop/marutsu/datasheet/ta7291.pdf)p.10より)

|IN1|IN2|OUT1|OUT2|備考|
|:-:|:-:|:--:|:--:|:--:|
|0  |0  |切断|切断|STOP|
|1  |0  |1   |0   |CW  |
|0  |1  |0   |1   |CCW |
|1  |1  |1   |1   |brake|

## 細々とした説明
### Serviceメッセージ
モータ番号とそのモータへの指令を渡すと成功か否かを返す．
一度に複数のモータに指令を送れるように配列になっている．
モータへの指令は-1.0(CCW)～1.0(CW)とする．

### モータへの指令からPWM信号への変換
モータへの信号は-1.0～1.0としている．
一方でPWMの信号はpigpioのgpioHardwarePWMを使用しているので0～1000000(1M)である．

回転方向はモータへの指令値から判定し，IN1とIN2の値を設定している．

回転速度はモータへの指令値の絶対値をPWM信号へ変換している．
Raspberry Piのgpio pinの出力は3.3Vである．一方，今回使用しているモータはタミヤの130タイプモータで適正電圧は1.5V～3.0Vっぽい．よってモータへの最大出力3.0Vを表すPWM信号のduty比は以下のようになる．

1000000 * 3.0 / 3.3 ≒ 909090

この辺りの処理をPiMotorControlNode::convMotorCommand関数に任せている．

# ビルド・実行
## ビルド
Raspberry Pi上でビルド．クロスコンパイルが面倒なので．

```terminal
ワークスペースのディレクトリに移動
$ colcon build --symlink-install
```

## 実行
Raspberry Pi上のターミナル二つで以下を実行．

```terminal:モータ制御用node実行用
$ sudo -i
ワークスペースに移動
# . install/setup.bash
# ros2 run pi_motor_control_nodes pi_motor_control_node __params:=pi_motor_control_nodes/yaml/pi_motor_control_node.yaml
```

pigpioはハードウェアを制御するためにsudo権限が必要となるのでsudo -iを行っている．
もしROS_DOMAIN_IDを使っているなら，sudoの後に設定し直すか/root/.bashrcにて設定する必要がある．

```terminal:nodeへの指令
$ . install/setup.bash
$ ros2 service call /pi_motor_control_node pi_motor_control_msgs/srv/MotorCommands "{target_motors: [0], motor_commands: [0.2]}"
$ ros2 service call /pi_motor_control_node pi_motor_control_msgs/srv/MotorCommands "{target_motors: [0], motor_commands: [-0.2]}"
$ ros2 service call /pi_motor_control_node pi_motor_control_msgs/srv/MotorCommands "{target_motors: [0], motor_commands: [0.0]}"
```

nodeへの指令は，とりあえずの方法としてコマンドで渡す．最初にCWの方向に20%(0.2)，CCWの方向に20%(-0.2)，最後に停止(0.0)としている．

### 実行結果

[![開けなかったらごめんなさい](https://i9.ytimg.com/vi/vjrHgD5sqSg/mq2.jpg?sqp=CJSZnesF&rs=AOn4CLBpyg6tKkrId39chLA0oJBTF2Pimg)](https://youtu.be/vjrHgD5sqSg)

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">ROS2とRaspberry Pi 3B+でモータ制御 <a href="https://t.co/klRYjmXfOD">https://t.co/klRYjmXfOD</a> <a href="https://twitter.com/YouTube?ref_src=twsrc%5Etfw">@YouTube</a>さんから</p>&mdash; くら けん (@nek009) <a href="https://twitter.com/nek009/status/1166919495316672518?ref_src=twsrc%5Etfw">August 29, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

# 参考
- [craftな毎日](https://iruchan.blog.so-net.ne.jp/_pages/user/iphone/article?name=2010-09-12)
    - PWMによるモータ制御としてはTB6549シリーズがよい？
- [トラ技 実践講座 小型モータの選定と制御技術 第8回 モータのスペック変更への対応 ～モータの応答性能を調べるテクニックも解説～](https://toragi.cqpub.co.jp/Portals/0/backnumber/2007/03/p235-236.pdf)
    - PWMによるモータ制御としてはTB6549シリーズがよい？
- [ロボコン用電子部品メモとリンク](http://www.ee.kochi-ct.ac.jp/~shiba/lib/devices/devices.html)
- [Ｈ８マイコンによるコントローラ（部品選定）](http://www.oit.ac.jp/is/L231/~whashimo/Article/HapticController/chap2.html)