WR-XX上の複数のサーボモータを制御するGUIのPythonコード（Tkinter Scale）

#はじめに

先日考えた平面３リンクマニピュレータの関節座標を求めるPythonプログラム(ROS用)をMS3Lに適用する際に、ふと、「プチロボ」のサーボモータの仕様ってそもそもどんなんだったかなと気になったの調べているうちに、ついでに、GUIのスライダーでサーボモータを動かしながら座標に当てはめたら面白いかもしれないと思い、Pythonには Tkinter Scaleというツールがあるのを思い出したので、初めて使ってみました。

##謎の「TowerPro製 SG91R」

私が購入したプチロボXL(WR-XL)には、Tower Pro社のMG90Sというマイクロサーボが付属していましたが、「WR-XX ハードウェア マニュアル（第２版）」には、「記載の接続サーボの最大個数の目安はプチロボ用サーボ『TowerPro SG91R』の場合です」とあり、またプチロボ”L”シリーズの拡張パーツ「サーボモータ１軸拡張セット WR-SG91R-SET」の「仕様」説明においてもサーボモータは「TowerPro製 SG91R」と書かれています。

そこで、SG91Rなるサーボモータ単体の仕様をネット上で探したのですが、そもそもTower Pro社のホームページ上にもSG91Rという製品の紹介がなくSG90しか見当たりませんでした。このシリーズの最新版は、「MG92R」という型番のようで、仕様上、ストール・トルクは2.5kg/cm(4.8v)もあるらしいです。MG90Sは1.8kg/cm(4.8v)で、SG91Rは「約2.0kg/cm(4.8v)」（WR-SG91R-SET仕様）となっているから、やはりそれぞれ別物なのだろうと思います。ただ、トルクが35%以上もアップしているというのは相当な違いのはず（ホントは、トルクはまだ未学習です・・・）なので「MG92R」はアマゾンでも購入できるのでいつか入手する機会があるといいなとは思います（「思う」だけならいくらでもタダなのです。）

##あらためてMG90Sの仕様をみてみる

この製品は、ネット上に海賊版の嵐が吹き荒れている中、「We are the original manufacturer of TowerPro MG90S servo」と宣言しているTower Pro社のサイト上にも、いわゆるデータシートレベルの仕様書は見当たりません。かろうじて、PDFになっている以下のURLが見つかりました。（ebayというフォルダに保存されているので怪しさは否定できませんが）

MG90S
Position "0" (1.5 ms pulse) is middle, "90" (~2 ms pulse) is all the way to the right, "-90" (~1 ms pulse) is all the way to the left.
https://www.epitran.it/ebayDrive/datasheet/MG90S.pdf

仕様がわかったのですが、今ひとつ使い方がわかりにくいので、愛読書の一つであるO'REILLYの「Raspberry Piクックブック」(2014/8)を調べてみました。この本は、1年ほど前に近所の古本屋で投げ売り価格で売られていたのを救済したもので、（特に、初代ラズパイを扱う際の）私の愛読書の一つです。サーボモータの制御についてのレシピが10.1にあります。素人が知りたい程度のことならナンデモカカッテコイッて感じの頼もしい本ですね。

サーボモーターの位置は、パルスの長さ（パルス幅）によって決まる。そして、サーボは少なくとも20ミリ秒に1回のパルスを期待している。パルスが１ミリ秒だけハイ[原文ママ]であれば、サーボの角度はゼロとなる。パルス幅が1.5ミリ秒であれば中央（90度）の位置に、パルス幅が２ミリ秒であれば180度の位置になる。（前述「クックブック」p.219）

ちなみに、前述の少し怪しげなURLのMG90Sの仕様書らしきで文書では、PWM Periodは20msで50Hzとなっているのでクックブックの記載とも符合しているのですが、クックブックのプログラム例では、PWM周波を100Hzに設定しているため、デューティー比を変換しているようです。PIC側がどんなプログラムになっているのか興味がワキます。

また、「クックブック」のレシピ9.8では、Tkinterユーザインタフェースフレームワークで、スライダーを使ってPWMデューティー比を変化させるプログラムが紹介されていました。これを少し改良したのが次に紹介するコードです。

#ソースコード

（動作画面）

###開発・利用環境

• Ubuntu 18.04LTS（PC:NEC LS550/F）
• Python 2.7
• VS Code
• ワンダーキットWR-XX/WS3L

###簡単なコメント

• Tkinterの使い方は、ネット上にいくつも紹介されていますのでわかりやすいです。ざっとGoogle検索して参考にしたサイトは以下のようなものです。特におすすめするという訳ではありませんが、役に立ちましたので参考として紹介します。
  • 上記「クックブック」のレシピ9.9は、スライダーを２つ以上使う場合の参考になります。
  • tutorialspointは、パラメータやオプションの意味を確認するのに一覧表形式で便利です。
  • Python Courseは、紹介されている直交座標形式でポイントを指定してアームを動かすのに使ったら確かに面白いと思いました。

gui_sliderWRXXforMS3L.py

#!/usr/bin/env python
from Tkinter import *
import serial, struct, time

# SET SERVO MOTION RANGE
MAX = 220 # up to 250
MIN = 1   # down to 1
# SET MOTION SPEED
SPEED = 8 # from 1 to 15
#SET MOTOR WAIT
WAIT = 1  # from 1 to 220 @15ms
# SET SERIAL PORT
DEVICE = '/dev/ttyUSB0'
# COMMAND 06 DATA FORMAT FOR MS3L (PC TX)
# [FD][11][06][WT][VL][M0][M1][M2][M3][M4][FD]
# **1/**2/**3/**4/**5/**6/**7/**8/**9/*10/*11/ 
cmd6 = [253,11,6,WAIT,SPEED,110,1,110,1,110,254]
cmd2 = [253,24,2] + [MAX] * 20 + [254]
cmd3 = [253,24,3] + [MIN] * 20 + [254]

def tx_command(command):
    for cmd in command:
        data = struct.pack("B", cmd)
        _serial.write(data)

class App:
	
    def __init__(self, master):
        frame = Frame(master)
        frame.pack()

        Label(frame, text='M0').grid(row = 0, column = 0)
        scaleM0 = Scale(frame, from_=MIN, to=MAX, length=300, tickinterval=MAX-1,
              orient=HORIZONTAL, command=self.updateM0)
        scaleM0.grid(row = 0, column = 1)
        scaleM0.set(110)

        Label(frame, text='M2').grid(row = 1, column = 0)
        scaleM2 = Scale(frame, from_=MIN, to=MAX, length=300, tickinterval=MAX-1,
              orient=HORIZONTAL, command=self.updateM2)
        scaleM2.grid(row = 1, column = 1)
        scaleM2.set(110)

        Label(frame, text='M4').grid(row = 2, column = 0)
        scaleM4 = Scale(frame, from_=MIN, to=MAX, length=300, tickinterval=MAX-1,
              orient=HORIZONTAL, command=self.updateM4)
        scaleM4.grid(row = 2, column = 1)
        scaleM4.set(110)

    def updateM0(self, position):
        cmd6[5] = int(position)
        tx_command(cmd6)

    def updateM2(self, position):
        cmd6[7] = int(position)
        tx_command(cmd6)

    def updateM4(self, position):
        cmd6[9] = int(position)
        tx_command(cmd6)

_serial = serial.Serial(DEVICE)
_serial.baudrate = 38400
_serial.parity = serial.PARITY_NONE
_serial.bytesize = serial.EIGHTBITS
_serial.stopbits = serial.STOPBITS_ONE
_serial.timeout = 0.1
_serial.flush()
print "serial device opened."

tx_command(cmd2)
tx_command(cmd3)

root = Tk()
root.title(u'WR-XX Servo Control for MS3L')
app = App(root)
root.geometry("350x250+0+0")
print "Command 6: " + str(cmd6)
root.mainloop()

#あとがき

このプログラムを書いてデバッグしているときに、実機に接続してもM2モータが動かず、ピクピクと電圧不足のような動作をしていました。どうしても、電源を入れたときに所定の位置に戻ろうとビクッと極端な動作が発生してしまうのです。これを防ぐためには、cmd6の初期設定値をモータに負荷がかかりにくい位置に設定すると少しはマシなようです。私の場合は、一直線に真上に並ぶ用にアーム位置を設定することにしました。その場合の値は、M0, M2, M4の順で72, 92, 100です。これは、最初にアームを組み立てたときのサーボホーンの位置で変わりますので、自分で調整して見つければ良いです。

ちなみに、Windowsを使っている場合は、ワンダーキットに付属しているもっと高機能な「WR-XX TEST.exe」というアプリケーションが製品に付属しています。
プチロボＬシリーズ　最新CD-ROM ダウンロードページからダウンロードしたファイルを解凍して、WR-MS3L_180425 > Program > WR-XX_TEST.exeというファイルを探してみてください。

今回は、ROS絡みのプログラムではありませんが、おもちゃにふさわしいGUIができてちょうどよい息抜きになりました。