Modbus通信でオリエンタルモーターROSパッケージをPythonで動かす

## はじめに

[株式会社Rist](https://www.rist.co.jp/)のロボットチームの原山です。
Ristは画像認識AIの会社ですが、その他にも色々とやっております。

今回は、社内のプロジェクトにおいてAGVに昇降機を付けて動かしています。
昇降機は、オリエンタルモーター株式会社の[EASシリーズEAS6RY-E085-ARMK-2](https://www.orientalmotor.co.jp/products/detail.action?hinmei=EAS6RY-E085-ARMK-2)を[Modubus-RS485\(COM-1PD)](https://www.contec.com/jp/products-services/daq-control/pc-helper/usb-module/com-1pd(usb)h/feature/)を使ってPythonで動かします。

## 環境
* [EASシリーズEAS6RY-E085-ARMK-2](https://www.orientalmotor.co.jp/products/detail.action?hinmei=EAS6RY-E085-ARMK-2)とその付属品
* PC: Ubuntu18.04
* [COM-1PD(USB)H](https://www.contec.com/jp/products-services/daq-control/pc-helper/usb-module/com-1pd(usb)h/feature/)

## モーターについて
[EASシリーズEAS6RY-E085-ARMK-2](https://www.orientalmotor.co.jp/products/detail.action?hinmei=EAS6RY-E085-ARMK-2)とその付属品の組み立てや設定に関しては、マニュアルがありますが、[メカ担当](https://twitter.com/MrBearing2)におまかせしているのでよくわかりません。私はPCとModbus(RS-485)を繋ぐだけで通信することができました。

## Modbus(RS-485)について

#### Modbus(モドバス)とは

Modbus(モドバス) とは、米Modicon社が自社PLC用に1979年に開発した通信プロトコルで、PLCのみならず電子機器間においてもデータ受け渡しの目的で使用されていて、一般的にModbusというとRS-232C、RS-422、RS-485通信のことを言うようです。

* 通信モードにはいくつかあって、今回のモーターはRTU(Remote Terminal Unit)モードで動かします。
USB接続なので下記必要です。

```
$ sudo su
# modprobe ftdi_sio vendor=0x06ce product=0x8331
# echo 06ce 8331 > /sys/bus/usb-serial/drivers/ftdi_sio/new_id
# exit
```
PCを再起動等すると毎回必要です。うまいことやってください。

## ドキュメント等
* **マニュアル**
    * ROSに関しての[マニュアル, ROSパッケージとサンプル](https://www.orientalmotor.co.jp/tech/connect/)があります。ログインアカウント必要です。

* **動作環境** 
    * 対象OS：Ubuntu16.04(Linux) -> Ubuntu18.04(Linux)でも動作確認できました。
    * ROSディストリビューション：Kinetic Kame　-> Melodic Moreniaでも動作確認できました。

### om_modbus_master
* [こちら](https://www.orientalmotor.co.jp/download/dialog/?serviceTypeNo=20&ninsyouFileId=465&senimotoUrl=%2Ftech%2Fconnect%2F&senimotoId=24)から"om_modbus_master"というROSパッケージを入手してください。

* om_modbus_masterをビルドしてください。

ROSノードを起動

```bash
$ source devel/setup.bash # ビルド直後なら必要
$ roslaunch om_modbus_master om_modbusRTU.launch com:="/dev/ttyUSB0" topicID:=1 baudrate:=115200 updateRate:=10 firstGen:="1,2," secondGen:="3,4,"
```
* connectedと表示されれば問題ありません。

### ROSメッセージ

プロジェクト直下のsampleフォルダにいくつかのサンプルコードがありますが、実際にモーターを動かすことはできませんでした。

* モーターへ運転データは、om_modbus_master/msg/om_query.msgを使用します。

また今回は、ダイレクト運転データモードかつ絶対位置決め方式です。メッセージの中身が変わります。
[専用ソフト](https://www.orientalmotor.co.jp/download/software/mexe02/)を使って、ソフトリミット設定済みです。

###### om_query.msg

| メンバ名 | 型 | 用途 | 備考 |
| -------- | -------- | -------- | -------- |
| slave_id | int8 | 号機番号 | モーターID |
| func_code | int8 | ファンクションコード | (0:read, 1:write, 2:read&write) |
| write_addr | int32 | 書き込みアドレス | **88**ダイレクトデータ運転アドレス |
| read_addr | int32 | 読み込みアドレス | **88**ダイレクトデータ運転アドレス |
| write_num | int8 | 書き込みのバイト数 | **8** |
| read_num | int8 | 読み込みのバイト数 | **8** |
| **data** | int32[32] | 下記参照 |  |


**data変数の内容**

| メンバ名 | 型 | 用途 |
| -------- | -------- | -------- |
|   data[0]   |   0   |   運転データナンバー   |
|   data[1]   |   1   |   方式は**絶対位置決め方式** の番号  |
|   data[2]   |   MIN～MAX   |   ソフトリミットで設定した値が**MIN**～**MAX**の値を指定できる  |
|   data[3]   |   2000   |  速度Hz    |
|   data[4]   |   1500   |  起動変化レート　kHz/s    |
|   data[5]   |   1500   |  指定レートkHz/s    |
|   data[6]   |   100   　|   電流   |
|   data[7]   |   1　　 　 |    反映トリガ 1:全データ反映  |
|   data[8]   |   7176   |   エラー(**固定**)  |


### サンプルを改良して動かす
**Python**

```python
#!/usr/bin/python
# -*- coding: utf-8 -*-

import rospy
import time
from om_modbus_master.msg import om_query
from om_modbus_master.msg import om_response
from om_modbus_master.msg import om_state

# グローバル変数
gState_driver = 0
gMotor_spd = 0

# レスポンス購読
def resCallback(res):
  global gMotor_spd
  if(res.slave_id == 1):
    gMotor_spd = res.data[0]

# ステータス購読
def stateCallback(res):
  global gState_driver
  gState_driver = res.state_driver

# 通信が終わるまで待機
def wait():
  global gState_driver
  time.sleep(0.01)
  while(gState_driver == 1):
    pass

def main():  
  rospy.init_node("sample1", anonymous=True)
  pub = rospy.Publisher("om_query1", om_query, queue_size=1)
  rospy.Subscriber("om_response1", om_response, resCallback)
  rospy.Subscriber("om_state1", om_state, stateCallback)
  msg = om_query()
  time.sleep(1)
  
  # 書き込み(回転速度No.2)
  msg.slave_id = 1
  msg.func_code = 1
  msg.write_addr = 88
  msg.write_num = 8
  msg.data[0] = 0
  msg.data[1] = 1
  msg.data[2] = 2000
  msg.data[3] = 2000
  msg.data[4] = 1500
  msg.data[5] = 1500
  msg.data[6] = 100
  msg.data[7] = 1
  msg.data[8] = 7176
  pub.publish(msg)
  wait()

  rospy.spin()

if __name__ == '__main__':
    main()
```

## おわりに
わかっていない部分も多いので間違っているところがあるかもしれないです。


# 参考
Modbus(RS-485)について
https://www.softech.co.jp/mm_110302_plc.htm
https://www.lineeye.co.jp/html/term_modbus.html

オリエンタルモーターのROS
https://www.orientalmotor.co.jp/tech/connect/