ブログ - KONDO PMX試食2

KONDO PMX試食2

カテゴリ : 
雑記
2024-4-16 14:20
前回に引き続きPMX。

各装置の接続は最終的にこのような構成に。
キャリアボードに供給する電源が内蔵のスイッチ回路を介してEHコネクタからサーボへ供給される。
Raspberry Pi Zero 2 Wの操作はモニタとキーボードを直接接続して行うも良し、USB経由のRNDISでリモートログインしても良し、WiFi経由でリモートログインしても良し。
これでハードウェアの準備は完了。

ではようやくソフトウェアに。
Raspberry Pi Zero 2 WをホストとしたのでLinuxが前提、Zeroは極めてメモリが少ないのでCLIを基本とした。今回はモータが動きさえすれば良いので、プログラムのデバッグはキャラクタベースで十分と判断。
キャリアボードはRaspberry Piの機能を利用するので、本家のドキュメントやDXHATの使用方法を元に設定。念のためcmdline.txtからconsoleの記述が消えているのを確認しておこう。
キャリアボードのRS-485 I/FはZeroの「/dev/ttyAMA0」に割り当てられる。
この辺は先人の知恵を借りれば良し。

次にPythonによるPMXとの通信だが、既にDXL用Pythonライブラリが公開されているのでその構成を真似ね、PMXのプロトコルで規定されているコマンドに応じた関数を用意し、こちらでpyPMX.pyとして公開。あると便利な検索・ID変更・ボーレート変更の各スクリプトも事のついでに作成。これにてオンラインマニュアル片手にポチポチと弄る準備が完了。
一応ライブラリの末尾にテストコードを仕込んでおいたが、PMXに位置決め制御を行わせるだけであれば、以下のように動作モードを設定しTorqueONした後にMotorWRITEで角度を指令するだけ。
#!/usr/bin/python3
import time
from pyPMX import PMXProtocol

ID = 0
BAUD = 3000000
# Create pmx instance
pmx = PMXProtocol('/dev/ttyAMA0', BAUD)

pmx.MemREAD(ID, 400, 6) # Error clear
pmx.MotorWRITE(ID, pmx.MOTW_OPT_FREE, ()) # Torque off
pmx.MemWRITE8(ID, 501, 1)  # bit 0:pos,1:speed,2:cur,3:torq,4:pwm,5:time
pmx.MotorWRITE(ID, pmx.MOTW_OPT_TORQUEON, ()) # Torque on

for ang in (0, 4500, 9000, 4500, 0, -4500, -9000, -4500, 0):
  pmx.MotorWRITE(ID, pmx.MOTW_OPT_NONE, (ang,)) # Command angle
  time.sleep(0.5)
pmx.MotorWRITE(ID, pmx.MOTW_OPT_FREE, ()) # Torque off
実はこの程度の処理にもかかわらず、想定した動作を行ってくれない事が散見された。各関数の成否を確認しないまま次の処理に遷移させているため、初期設定の所で何かしら失敗したまま角度の指令がなされている時に動かなかった様だ。
それがキャリアボードの問題なのか通信方法の問題なのかの切り分けはしていないが、関数の返り値を確認してリトライする等の処理を加えれば、とりあえずお茶は濁せそうだ。

それらを意識した上で複数IDを一括で処置する関数を作成、少しだけコンソール上での操作をしやすくするためにkbhitを導入、最終的に2軸を対象にしたプログラムを作ってみた。
#!/usr/bin/python3

_retrynum = 3
# mode set & torque enable
def _setmode(pmx, id, m):
  for i in range(_retrynum):
    if pmx.MemWRITE8(id, 500, pmx.MOTW_OPT_FREE):
      if m != 0:
        # 1:pos,2:speed,4:cur,8:torq,16:pwm,32:time
        if pmx.MemWRITE8(id, 501, m):
          if pmx.MemWRITE8(id, 502, 0b11111):
            if pmx.MemWRITE8(id, 503, 5):
              if pmx.MemWRITE8(id, 500, pmx.MOTW_OPT_TORQUEON):
                return True
      else:
        return True
  return False

def SetMode(pmx, ids, m):
  if isinstance(ids, list) or isinstance(ids, tuple):
    r = 0
    for i, id in enumerate(ids):
      if isinstance(m, list) or isinstance(m, tuple):
        r += 1 if _setmode(pmx, id, m[i]) else 0
      else:
        r += 1 if _setmode(pmx, id, m) else 0
    return True if r == len(ids) else False
  else:
    return _setmode(pmx, ids, m)

# set angle
def _setangle(pmx, id, ang, t):
  for i in range(_retrynum):
    r = pmx.MotorWRITE(id, pmx.MOTW_OPT_NONE, (int(ang * 100), t))
    if r != None:
      if len(r) > 0:
        return r[1]
  return ()

def SetAngle(pmx, ids, angs, t):
  if isinstance(ids, list) or isinstance(ids, tuple):
    r = ()
    for i, id in enumerate(ids):
      if isinstance(angs, list) or isinstance(angs, tuple):
        r += _setangle(pmx, id, angs[i], t),
      else:
        r += _setangle(pmx, id, angs, t),
    return r
  else:
    return _setangle(pmx, ids, angs, t)

# set velocity
def _setvelocity(pmx, id, velo):
  for i in range(_retrynum):
    r = pmx.MotorWRITE(id, pmx.MOTW_OPT_NONE, (int(velo),))
    if r != None:
      if len(r) > 0:
        return r[1]
  return ()

def SetVelocity(pmx, ids, velos):
  if isinstance(ids, list) or isinstance(ids, tuple):
    r = ()
    for i, id in enumerate(ids):
      if isinstance(velos, list) or isinstance(velos, tuple):
        r += _setvelocity(pmx, id, velos[i]),
      else:
        r += _setvelocity(pmx, id, velos),
    return r
  else:
    return _setvelo(pmx, ids, velos)

def _getpresent(pmx, id):
  for i in range(_retrynum):
    r = pmx.MemREAD16(id, 300, 5, signed = True)
    if r != None:
      return r
  return ()

def GetPresent(pmx, ids):
  if isinstance(ids, list) or isinstance(ids, tuple):
    r = ()
    for i, id in enumerate(ids):
      r += _getpresent(pmx, id),
    return r
  else:
    return _getpresent(pmx, ids)

if __name__ == "__main__":
  import os, sys, time, kbhit
  from pyPMX import PMXProtocol

  kb = kbhit.KBHit()
  pmx = PMXProtocol('/dev/ttyAMA0', 3000000, 0.005)

  ID1 = 0
  ID2 = 1

  # Position Control
  print('Position Control')
  print(f'{ID1}:angle velo   cur   torque  pwm   {ID2}:angle velo   cur   torque  pwm')
  if SetMode(pmx, (ID1, ID2), 1|32):
    while not kb.kbhit():
      t = 1000
      for ang in (0, 45, 90, 45, 0, -45, -90, -45):
        if kb.kbhit(): break
        SetAngle(pmx, (ID1, ID2), (ang, ang * 2), t)
        s = time.time() + t / 1000.0
        while s > time.time():
          if kb.kbhit(): break
          r = GetPresent(pmx, (ID1, ID2))
          for r in r:
            print(('{:6d} '*len(r)).format(*r), end='')
          print(end='\r')
    SetMode(pmx, (ID1, ID2), 0)
    kb.getch()

  # Velocity Control
  print('\nVelocity Control')
  print(f'{ID1}:angle velo   cur   torque  pwm   {ID2}:angle velo   cur   torque  pwm')
  if SetMode(pmx, (ID1, ID2), 2):
    while not kb.kbhit():
      t = 3000
      for velo in (0, 500, 1000, 1500, 1000, 500, 0, -500, -1000, -1500, -1000, -500):
        if kb.kbhit(): break
        SetVelocity(pmx, (ID1, ID2), (velo*1, velo*1.5))
        s = time.time() + t / 1000.0
        while s > time.time():
          if kb.kbhit(): break
          r = GetPresent(pmx, (ID1, ID2))
          for r in r:
            print(('{:6d} '*len(r)).format(*r), end='')
          print(end='\r')
    SetMode(pmx, (ID1, ID2), 0)

  del kb, pmx
適当な指令値を元に位置決め制御と速度制御を行い、コンソール上で何か入力すると処理を推移する。制御中は読み出した現在値をバラバラとコンソールに吐き出し続ける。
紹介したコードはいらぬものも含んでいるので少々長くなっているが、SetModeを行って成功したらSetAngleやSetVelocityを行っているだけの事である。

Zeroを一通り操作している様子をラズパイカメラで自撮りしておいた。

ちなみに先の関数エラーはRS-485トランシーバの電気的な問題だったようで、真面目にターミネータの他にプルアップとプルダウン抵抗を装備して事なきを得た。

技術

トラックバック

トラックバックpingアドレス http://www.besttechnology.co.jp/modules/d3blog/tb.php/249