ブログ - PMXネタつながりで

PMXネタつながりで

カテゴリ : 
雑記
2024-4-25 9:50
事のついでで恐縮だが、キャリアボードの評価をお願いしている方から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)
今回はRZ/V2LのDRP-AIを評価したベンチで使ったサーボをZero 2 Wでも試してみたに過ぎないが、こんな具合に使える(長いだけで大して面白くないので再生するまでもなく)。

事のついでと書いている大本の理由は、ここ最近XL330/XC330が軒並み通信できないベンチに遭遇して痛い目にあっており、動かすチャンスがあるときは一通り試しておかないと気が気でないものだから(XC330/XL330はボーレートの許容誤差が狭い)。

技術

トラックバック

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