事のついでで恐縮だが、キャリアボードの評価をお願いしている方からKO B3Mがマトモに動かないという指摘があったので、そのついでに作っておいた。10年前にこねくり回すのを諦めてから重い腰を上げて弄ったが、結局のところB3Mの癖は拭い切れないまま。

こちらも事のついでだが、少し毛色の違うサーボを引っ張り出して触っておいたレポートを。

外観が何かに酷似しているのは気のせいとして、サーボモータとして最低限の制御を行うついでに、テキストベースのプロトコルを被せているところが毛色の違いかと。ようするに俺サーボ。
適当なシリアルターミナル上でポチポチキーボードを打てばとりあえず動くが、プログラムからテキストで通信させるとなると却って面倒。こちらも例に漏れずPythonで記述するためのクラスが用意されているが、シリアル通信部分を隠蔽するだけで実質何もしておらず、結局正規表現任せ。

これを2個使い相互リンクをやらせてみた(単に互いの位置偏差を電流指令しているだけだが)。実機を持つ方はたかが知れているのでコードを晒すまでもないが、2軸への電流制御モード設定・ゲイン設定・電流指令・現在値取得が1パケットで済み、要点のみであれば実質数行で済む(PeriodicTimerは周期タイマクラス)。

#!/usr/bin/python3
#
# Link two servos with virtual wires

import os, platform, psutil, time, kbhit, serial, numpy as np
from PeriodicTimer import PeriodicTimer
from jp200class import packet

intervalms = 20

try:
  pr = psutil.Process(os.getpid())
  if platform.system() == 'Windows':
    pr.nice(psutil.HIGH_PRIORITY_CLASS)
  else:
    pr.nice(-10)
except:
  print('info:run with sudo')

# present value
present_dt = [('ang',float),('cur',float),('velo',float)]
present = np.empty(2, dtype = present_dt)
# calculated current command value
tcur = [0.0] * 2

#--------------------------------
# Periodically executed functioin
#--------------------------------
def cycfunc(*p):
  sv = p[0]
  acks = []
  # deviation from each other's position
  tcur = (
    np.clip((present[1]['ang'] - present[0]['ang']) * 0.15, -1000.0, 1000.0),
    np.clip((present[0]['ang'] - present[1]['ang']) * 0.15, -1000.0, 1000.0)
  )
  # send current command and current position acquisition to JP200
  sv.request(
    '#0EX=4SG2=9000;5500;10TC=%dTP=1000CACVCC'
    '#1EX=4SG2=9000;5500;10TC=%dTP=1000CACVCC'
    %(tcur[0], tcur[1]), acks
  )
  # analyze response
  if acks:
    for ack in acks:
      m = sv.p2.findall(ack)
      id = -1
      for n in m:
        try:
          if n[0] == '#' and int(n[1]) >= 0: id = int(n[1])
          elif n[0] == 'CA' and id >= 0: present[id]['ang'] = float(n[1])
          elif n[0] == 'CC' and id >= 0: present[id]['cur'] = float(n[1])
          elif n[0] == 'CV' and id >= 0: present[id]['velo'] = float(n[1])
        except:
          pass
      if id >= 0: print('[%d] %4d %8d %5d %5d  '%(id, tcur[id], present[id]['ang'], present[id]['velo'], present[id]['cur']), end = '')
    print(end = '\r')

# instantiate kbhit
kb = kbhit.KBHit()
# open UART
ser = serial.Serial('/dev/ttyAMA0', 2000000)
print(ser.name)
# instantiate JP200
sv = packet(ser, 0.02)
# instantiate periodic timer & start
tm = PeriodicTimer(intervalms / 1000, cycfunc, [sv])

acks = []
k = ''
while k != 'q':
  # waiting for key input
  try:
    k = kb.getch()
  except UnicodeDecodeError:
    pass
  print(k, end = '\n', flush = True)

  # move to origin position
  if k == 'h':
    tm.stop()
    sv.request('#0EX=1TA=18000TP=1000#1EX=1TA=18000TP=1000', acks)
  # stop timer
  elif k == 's':
    tm.stop()
    tm.start()
  # start timer
  elif k == 'e':
    tm.stop()
    sv.request('#0EX=0#1EX=0', acks)
  elif k == 'c':
    tm.stop()
    sv.request('#0EX=8TP=1000#1EX=8TP=1000', acks)
    time.sleep(5)
    sv.request('#0EX=8TP=-1000#1EX=8TP=-1000', acks)
    time.sleep(5)
    sv.request('#0EX=0#1EX=0', acks)
  else:
    print('XXXXXXXXX')
tm.stop()

sv.request('#0EX=0#1EX=0', acks, True)
ser.close()
kb.set_normal_term()
del sv, ser, kb

os._exit(0)

技術