二足歩行ロボット研修（kora編）[12] DYNAMIXEL SDKでサーボと送受信する

こんにちは。koraです。

今回はRaspberry Pi 4のGPIOピンからRS485通信の送信と受信に挑戦します。
実はやってみてわかったのですがかなり面倒なところが多いです。搭載スペースに余裕がある場合はROBOTISのUSB通信コンバータU2D2を使えば時間の節約になりますのでそちらをおすすめします。

UART3を使えるようにする

Raspberry Pi公式のUART解説によると、Raspberry Pi 4のGPIOピンヘッダにデフォルトで繋がっているUART1は、mini UARTという機能縮小版らしく、本来のUART0はBluetoothに繋がっているそうです。設定で入れ替えることもできるそうですが、Bluetoothの機能はもとのまま残しておきたいので、ここではRaspberry Pi 4で新たに追加されたというUART2~UART5のうちのUART3を使うことにします。

UART3はデフォルトでは無効になっているので、/bootディレクトリの設定ファイルで有効にします。RaspberryPi OSでは/boot/config.txtファイルを編集しますが、Ubuntu 18.04では/boot/firmware/usercfg.txtを編集すればいいようです。設定項目はこちらで確認できます。このファイルに

dtoverlay=uart3,ctsrts

を追加して再起動させます。

上記の設定でUART3が/dev/ttyAMA1として使えるようになりました。

接続するGPIOピンは次のとおりです。

• TX: GPIO4
• RX: GPIO5
• RTS: GPIO7
• 5V
• GND

ちなみにRaspberry PiのGPIOは3.3V系なので、RS485変換ICが5V系の場合はRXに接続する信号線が3.3Vを超えないようレベルコンバータを追加するか、分圧するなどの対策が必要です。

RS485用のRTS信号を出せるようにする

前々回の記事で解説したように、RS485は送信と受信を切り替える必要があります。
DYNAMIXEL SDKのPython APIは内部でpySerialというPythonでシリアル通信を可能にするモジュールが使われています。pySerialではRS485がサポートされていて、シリアル端子のRTSピンで送受信切り替え信号を出すことができます。が、どうやらこの切り替えには数msの遅延が発生していて、サーボから返ってくるデータを取りこぼしてしまうようです。

参考記事 How to Use a Raspberry Pi and Custom RS485 HAT With Modbus Protocol

仕方がないので自分で切り替えるようにします。

pySerialの変更

pySerialのSerialクラスうち、変更が必要な部分はwriteメソッドだけです。そこで、Serialクラスを継承してwriteメソッドをオーバーライド（書き換え）します。上記の参考記事をもとに新しいクラスを作りました。

# https://gitlab.com/wolutator/modbusmaster/-/blob/master/LICENSE

import serial
import serial.serialutil
import fcntl
import termios
import os
import struct

class RS485Ext(serial.Serial):
    def __init__(self, *args, **kwargs):
        super(RS485Ext, self).__init__(*args, **kwargs)

    def write(self, b):
        # set RTS pin high
        fd = self.fileno()
        buf = struct.pack('<1i', termios.TIOCM_RTS)
        fcntl.ioctl(fd, termios.TIOCMBIC, buf) # clear

        # write data
        d = serial.serialutil.to_bytes(b)
        length = os.write(self.fileno(), d)

        # wait for TX
        lsr = struct.pack('<1i', 0)
        while(True):
            # check line status register
            unpacked_lsr = struct.unpack('<1i',
                fcntl.ioctl(fd, termios.TIOCSERGETLSR, lsr))
            if (unpacked_lsr[0] & termios.TIOCSER_TEMT):
                break

        # set RTS pin low
        fcntl.ioctl(fd, termios.TIOCMBIS, buf) # set

        return length

ioctlとはデバイスを制御するシステムコール（OSの機能を呼び出すインターフェース）です。fcntlというモジュールを使えばPythonから利用できます。
ioctlに渡す引数はtermiosモジュールの定数を使います。TIOCM_RTSがRTSピンの状態を示すビットで、TIOCMBISがビットをセットするリクエスト、TIOCMBICがビットをクリアするリクエストです。
通常RTSは負論理なので、セットするとLowになりクリアするとHighになります。つまり送信を開始する前にリセットして送信が終わったらセットすることで送受信を切り替えられます。
なお送信が終わったかどうかはTIOCSERGETLSRというリクエストで返ってくる値のTIOCSER_TEMTビットで確認します。

DYNAMIXEL SDKの変更

変更した新しいクラスをDYNAMIXEL SDKと一緒に利用できるようにします。変更が必要なのはport_handlerモジュールのsetupPortメソッドだけなので、先ほどと同様にオーバーライドします。

import serial
import serial_rs485
from dynamixel_sdk import PortHandler

class PortHandlerExt(PortHandler):
    def __init__(self, *args, **kwargs):
        super(PortHandlerExt, self).__init__(*args, **kwargs)

    def setupPort(self, cflag_baud):
        if self.is_open:
            self.closePort()

        self.ser = serial_rs485.RS485Ext(
            port=self.port_name,
            baudrate=self.baudrate,
            bytesize=serial.EIGHTBITS,
            timeout=0
        )

        self.is_open = True
        self.ser.reset_input_buffer()
        self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0

        return True

これで、DYNAMIXEL XM540と送受信できるようになり、サーボの現在位置などを取得できるようになりました。

移動中の現在位置を取得してみた例