モーターの動作確認

MEXE02 によるモータードライバーの初期設定

MEXE02 を用いて初期設定を行う.モータードライバーの USB Micro-B と PC を接続し,モータードライバーに電源を投入する.MEXE02 を起動し,ポートを選択する.

右車輪駆動用のモータードライバーを id1,左車輪駆動用のモータードライバーを id2 と設定する．詳しい設定ファイルは以下にある．書き込みが終わり次第電源を切って再起動する.

書き込む内容は通信用 id や通信速度，位置・速度・加速度の単位設定などである．

Windows 経由，Modbus Tools を使用した通信テスト

Modbus Tools を用いて通信テストを行う.先ほど刺した USB ケーブルを抜き,COM-1PD(USB)H を接続する.POWER の LED が白色に点灯していることを確かめる.デバイスマネージャーを開き,COM ポート番号を確認する.認識されていない場合は配線がうまくいっていない可能性が高い． Modbus Tools を開いて通信設定を開く.RTU を選択し,先ほど確認した COM ポートに接続する.このとき通信設定を設定したものに揃える.ポートがオープンしているかを確認する.

以下を送信しレスポンスが返ってくることを確認する.正常の場合,緑色でレスポンスが帰ってくる.モーターからは S-ON をオンにしたためジーーーという音が聞こえてくる. タイムアウトが発生する場合は配線やソフトの設定がうまくいっていない可能性が高い．

id1 のモータードライバーの S-ON を立ち上げるコマンド

01 10 00 7C 00 02 04 00 00 00 01 35 1E 00 00 00 00

上記を実行したうえで以下を送信すると回転を開始する.

id1 のモータードライバーで回転数一定加速度一定で回転させるコマンド

01 10 00 5A 00 0E 1C 00 00 00 30 00 00 00 00 00 00 13 E8 00 00 03 E8 00 00 09 C4 00 00 03 E8 00 00 00 01 87 36 C2 93 00 00 00 00 00 00

以下を送信すると回転を停止する.

id1 のモータードライバー回転数を 0 にするコマンド

01 10 00 5A 00 0E 1C 00 00 00 00 00 00 00 00 00 00 00 00 00 00 03 E8 00 00 09 C4 00 00 03 E8 00 00 00 01 46 B9 00 00 00 00 00 00

回転が開始しない場合について述べる.まずモータードライバーに適切な電圧がかかっているかを確認する.PWR/SYS が白色に点灯していれば問題ない. USB-RS485 変換器の POWER が点灯しているか確かめる.次に送信を押した瞬間に Tx の LED が点灯しているかを確認する.点灯していれば信号自体は送られている可能性が高い.次に送信を押して Tx が点灯した次の瞬間に Rx が点灯するかを確認する.信号が問題なく帰っていれば点灯する.ここが点灯しているのに回転しない場合は送信している指令を間違えている可能性が高いため見直す. Rx が点灯しなかった場合はモータードライバーの COMM が点灯しているかを確かめる.白色点灯であれば正常,赤色点灯であれば異常である.赤色点灯の場合は Tx と Rx の逆接などが原因としてあり得る.そもそも点灯しなかった場合は信号が未達か,通信用電源が正常でない可能性が高いので電気的な接続,電源電圧を確かめる.

Ubuntu 経由，Python を使用した通信テスト

以下を実行する.

sudo su
modprobe ftdi_sio vendor=0x06ce product=0x8331
echo 06ce 8331 > /sys/bus/usb-serial/drivers/ftdi_sio/new_id
exit

これによって COM-1PD(USB)H が認識されるはずである.

dmesg | grep ttyUSB

を実行して製品名を確認できれば問題ない.

sudo chmod 777 /dev/ttyUSB0

でデバイスの読み書きの権限を与える.

pyserial をインストールしておく.

pip install pyserial

Python の pyserial を用いて通信テストを行う.コードは以下の用になる.

import serial
import array
import time

client = serial.Serial("/dev/ttyUSB0", 9600, timeout=0.01, parity=serial.PARITY_EVEN,stopbits=serial.STOPBITS_ONE) # ポートはttyUSB0,通信速度は9600bps,パリティは偶数,ストップビットは1
print(client.name)
size = 8

query_start = bytes([0x01,0x10,0x00,0x7c,0x00,0x02,0x04,0x00,0x00,0x00,0x01,0x35,0x1E,0x00,0x00,0x00,0x00])#モーターのS-ONをオンにする要求.intの配列をbytes関数に渡してバイト列を作る

query_spin = bytes([0X01,0x10,0x00,0x5A,0x00,0x0E,0x1C,0x00,0x00,0x00,0x30,0x00,0x00,0x00,0x00,0x00,0x00,0x13,0xE8,0x00,0x00,0x03,0xE8,0x00,0x00,0x09,0xC4,0x00,0x00,0x03,0xE8,0x00,0x00,0x00,0x01,0x87,0x36,0xC2,0x93,0x00,0x00,0x00,0x00,0x00,0x00]) # モーターを回転させる要求

print(query_start)
client.write(query_start)
result = client.read(8) # レスポンスを待つ

time.sleep(5) #S-ONがオンになってジーーという音が鳴ることを確認する

print(query_spin)
client.write(query_spin)
result = client.read(8) # レスポンスを待つ

これで回転することが確認できたら次の段階に移る．

ROS から回転させる

公式から ROS のドライバーをダウンロードしてくる. 解凍して~/catkin_ws/src/の中に配置する. 以下を実行してビルドする.

cd ~/catkin_ws
catkin build

とくにエラーが出ずに完了すれば次に進む．たまにできないのでもう 1 回繰り返すとできる.

roslaunch om_modbus_master om_modbusRTU.launch com:="/dev/ttyUSB0"  baudrate:=9600 secondGen:="1,2"

で ROS ノードを立ち上げる.com でポート,baudrate で通信速度,secondGen で号機番号(id)を指定する.(今回使用した BLV-R は第二世代なので secondGen を使用した).connent が表示されれば ROS 側での設定は問題ないと考えられる.ただし,ポートさえ合っていればコネクトと表示されるようなので注意が必要. ここで設定する値は MEXE02 で設定した値にそろえる。Launch ファイルに値を書き込んでおくといちいち指定しなくてよい。

rosrun om_modbus_master sample.py

で実行する.接続が問題なければ回転を開始する.以上で導入終わり.

#!/usr/bin/python3
# -*- 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_query0", om_query, queue_size=1)
  rospy.Subscriber("om_response0", om_response, resCallback)
  rospy.Subscriber("om_state0", om_state, stateCallback)
  msg = om_query()
  time.sleep(1)

  # 運転指令(S-ONをONする)
  msg.slave_id = 1        # スレーブID
  msg.func_code = 1       # ファンクションコード: 0:Read 1:Write 2:Read/Write
  msg.write_addr = 124    # アドレス指定： ドライバ入力指令
  msg.write_num = 1       # 書き込みデータ数: 1
  msg.data[0] = 1         # S-ONを立ち上げる
  pub.publish(msg)        # 配信
  wait()

  time.sleep(5)           # 5秒待機

  # 回転開始指令(ダイレクトドライブモード)
  msg.slave_id = 1
  msg.func_code = 1
  msg.write_addr = 90
  msg.write_num = 7
  msg.data[0] = 48        # DDO運転方式 16:連続運転(速度制御)
  msg.data[1] = 0         # DDO運転位置(初期単位：1step = 0.01deg)連続運転(速度制御)なので無関係
  msg.data[2] = 4000      # DDO運転速度(初期単位：r/min)
  msg.data[3] = 1000      # DDO加速レート(初期単位：ms)
  msg.data[4] = 1000      # DDO減速レート(初期単位：ms)
  msg.data[5] = 1000      # トルク制限値 (100.0%)
  msg.data[6] = 1         # DDO運転トリガ設定
  pub.publish(msg)        # 配信
  wait()

  time.sleep(10)          # 10秒待機

  # 回転停止指令
  msg.slave_id = 1
  msg.func_code = 1
  msg.write_addr = 90
  msg.write_num = 7
  msg.data[0] = 48        # DDO運転方式 16:連続運転(速度制御)
  msg.data[1] = 0         # DDO運転位置(初期単位：1step = 0.01deg)連続運転(速度制御)なので無関係
  msg.data[2] = 0         # DDO運転速度(初期単位：r/min)
  msg.data[3] = 1000      # DDO加速レート(初期単位：ms)
  msg.data[4] = 1000      # DDO減速レート(初期単位：ms)
  msg.data[5] = 1000      # トルク制限値 (100.0%)
  msg.data[6] = 1         # DDO運転トリガ設定
  pub.publish(msg)        # 配信
  wait()

  time.sleep(5)           # 5秒待機

# 運転指令(S-ONをOFFする)
  msg.slave_id = 1        # スレーブID
  msg.func_code = 1       # ファンクションコード: 0:Read 1:Write 2:Read/Write
  msg.write_addr = 124    # アドレス指定： ドライバ入力指令
  msg.write_num = 1       # 書き込みデータ数: 1
  msg.data[0] = 0         # S-ONを立ち下げる
  pub.publish(msg)        # 配信
  wait()


if __name__ == '__main__':
    main()