 |
 |
 |
 |
テストコード 
Python前提。
B3Mの挙動の再現性が乏しくなることが頻出するので、正直なところ参考程度に考えておいてほしい。
Python
B3Mはパケット構成が緩いので、ファームウェアが想定していない条件で通信を行うと重篤な事態に陥りやすい。
WRITEとPOSITIONのみマルチに対応したコマンドとしてWRITEsとPOSITIONsを用意。
ERROR STATUSをどのように成否判定に使うべきかが判然としないので無視。
#!/usr/bin/python3
# -*- coding: utf-8 -*-
#
# pyB3M.py
# SPDX-License-Identifier: MIT
# SPDX-FileCopyrightText: (C) 2024 mukyokyo
import serial, threading, array, struct
from typing import Union
from collections import namedtuple
from struct import pack, unpack, iter_unpack
from typing import overload
##########################################################
# Functionalized the part of converting int to bytes.
# If specified as a tuple, it is converted to bytes at once.
##########################################################
def B2Bs(d) -> bytes:
if isinstance(d, list) or isinstance(d, tuple):
return bytes(((d & 0x7f) | 0x80) if d < 0 else d for d in d)
else:
return bytes(pack('<B', ((d & 0x7f) | 0x80) if d < 0 else d))
def W2Bs(d) -> bytes:
if isinstance(d, list) or isinstance(d, tuple):
return b''.join([pack('<H',d) for d in [((d & 0x7fff) | 0x8000) if d < 0 else d for d in d]])
else:
return bytes(pack('<H', ((d & 0x7fff) | 0x8000) if d < 0 else d))
def L2Bs(d) -> bytes:
if isinstance(d, list) or isinstance(d, tuple):
return b''.join([pack('<I',d) for d in [((d & 0x7fffffff) | 0x80000000) if d < 0 else d for d in d]])
else:
return bytes(pack('<I', ((d & 0x7fffffff) | 0x80000000) if d < 0 else d))
##########################################################
# API for Kondo B3M
##########################################################
class B3MProtocol:
BROADCASTING_ID = 0xff
OPT_LOAD = 1
OPT_SAVE = 2
OPT_READ = 3
OPT_WRITE = 4
OPT_RESET = 5
OPT_POSITION = 6
(SYSW_BAUD_57600, SYSW_BAUD_115_2k, SYSW_BAUD_625k, SYSW_BAUD_1M, SYSW_BAUD_1_25M, SYSW_BAUD_1_5M, SYSW_BAUD_2M, SYSW_BAUD_3M) = range(8)
(SYSW_PARITY_NONE, SYSW_PARITY_ODD, SYSW_PARITY_EVEN) = range(3)
(MOTW_OPT_NONE, MOTW_OPT_TORQUEON, MOTW_OPT_FREE, _, MOTW_OPT_BRAKE, _, _, _, MOTW_OPT_HOLD) = range(9)
def __init__(self, port : Union[serial.Serial, str], baudrate = 57600, timeout = 0.05, lock = None):
if isinstance(port, serial.Serial):
self.__serial = port
self.__baudrate = port.baudrate
self.__timeout = port.timeout
else:
self.__serial = serial.Serial(port, baudrate = baudrate, timeout = timeout)
self.__baudrate = self.__serial.baudrate
self.__timeout = self.__serial.timeout
if lock == None:
self.__lock = threading.Lock()
else:
self.__lock = lock
self.__status = 0
def __del__(self):
pass
@property
def lock(self):
return self.__lock
@property
def baudrate(self):
return self.__serial.baudrate
@baudrate.setter
def baudrate(self, baudrate):
self.__baudrate = baudrate
self.__serial.baudrate = baudrate
@property
def timeout(self):
return self.__serial.timeout
@timeout.setter
def timeout(self, timeout):
self.__timeout = timeout
self.__serial.timeout = timeout
def __reconfig(self):
self.__serial.baudrate = self.__baudrate
self.__serial.timeout = self.__timeout
@property
def status(self):
return self.__status
def TxPacket(self, cmd : int, opt : int, param : bytes, echo = False) -> (bytes, bool):
self.__reconfig()
if cmd >= 0 and cmd <= 0xff and opt >= 0 and opt <= 0xff and len(param) <= (256 - 5):
txp = bytes((len(param) + 4, cmd, opt)) + bytes(param)
txp += B2Bs(sum(txp) & 0xff)
self.__serial.reset_input_buffer()
if echo: print('TX:', txp.hex(':'))
self.__serial.write(txp)
return txp, True
return None, False
def __rx(self, length) -> bytes:
s = self.__serial.read(length)
l = len(s)
if l == length:
return s
else:
r = s
length -= l
while self.__serial.in_waiting > 0:
s = self.__serial.read(length)
r += s
length -= len(s)
if length <= 0:
break
return r
def RxPacket(self, echo = False) -> (bytes, bool):
rxp = self.__rx(1)
if rxp:
if len(rxp) == 1:
if rxp[0] >= 5:
l = rxp[0] - 1
rxp += self.__rx(l)
if rxp[1] >= 0x81 and rxp[1] <= 0x86 and rxp[3] <= 0xfe and rxp[-1] == (sum(rxp[:-1]) & 0xff):
self.__status = rxp[2]
if echo: print('RX:', rxp.hex(':'))
return bytes(rxp), True
if echo: print('RX;', rxp.hex(';'), ' xxx')
return None, False
def WRITE(self, id : int, addr : int, data : bytes, reset = False, echo = False) -> bool:
with self.__lock:
if id >= 0 and id <= 255 and addr >= 0 and addr <= 0xff and len(data) <= 256 - 7:
if self.TxPacket(self.OPT_WRITE, 0x80 if reset else 0x00, B2Bs(id) + data + B2Bs(addr) + B2Bs(1), echo)[1]:
if id != self.BROADCASTING_ID:
d, r = self.RxPacket(echo)
if r:
return (d[3] == id) and (d[1] == (self.OPT_WRITE | 0x80))
else:
return True
return False
def WRITEs(self, addr : int, iddatas : Union[list, tuple], reset = False, echo = False) -> bool:
with self.__lock:
if addr >= 0 and addr <= 0xff and (isinstance(iddatas, list) or isinstance(iddatas, tuple)):
cnt = len(iddatas)
if cnt > 0:
txd = bytes(0)
l = len(iddatas[0])
for iddata in iddatas:
if iddata[0] < 0 or iddata[0] > 254 or len(iddata) != l:
return False
txd += iddata
txd += B2Bs(addr) + B2Bs(cnt)
return self.TxPacket(self.OPT_WRITE, 0x80 if reset else 0x00, txd, echo)[1]
return False
def WRITE8(self, id : int, addr : int, data : Union[int, list, tuple], reset = False, echo = False) -> bool:
return self.WRITE(id, addr, B2Bs(data), reset, echo)
def WRITE16(self, id : int, addr : int, data : Union[int, list, tuple], reset = False, echo = False) -> bool:
return self.WRITE(id, addr, W2Bs(data), reset, echo)
def WRITE32(self, id : int, addr : int, data : Union[int, list, tuple], reset = False, echo = False) -> bool:
return self.WRITE(id, addr, L2Bs(data), reset, echo)
def READ(self, id : int, addr : int, length : int, reset = False, echo = False) -> bytes:
with self.__lock:
if addr >= 0 and id <= 254 and addr >= 0 and addr <= 0xff and length >= 1 and length <= 250:
if self.TxPacket(self.OPT_READ, 0x80 if reset else 0x00, B2Bs(id) + B2Bs(addr) + B2Bs(length), echo)[1]:
d, r = self.RxPacket(echo)
if r:
if d[1] == (self.OPT_READ | 0x80) and id == d[3] and len(d) == length + 5:
return bytes(d[4:-1])
return None
def READ8(self, id : int, addr : int, length = 1, signed = False, reset = False, echo = False) -> int:
r = self.READ(id, addr, length, reset, echo)
if r != None:
n = sum(iter_unpack('b' if signed else 'B', r), ())
return n if length > 1 else n[0]
return None
def READ16(self, id : int, addr : int, length = 1, signed = False, reset = False, echo = False) -> int:
r = self.READ(id, addr, 2 * length, reset, echo)
if r != None:
n = sum(iter_unpack('h' if signed else 'H', r), ())
return n if length > 1 else n[0]
return None
def READ32(self, id : int, addr : int, length = 1, signed = False, reset = False, echo = False) -> int:
r = self.READ(id, addr, 4 * length, reset, echo)
if r != None:
n = sum(iter_unpack('i' if signed else 'I', r), ())
return n if length > 1 else n[0]
return None
def LOAD(self, id : int, echo = False) -> bool:
with self.__lock:
if self.TxPacket(self.OPT_LOAD, 0x0, B2Bs(id), echo)[1]:
if id != self.BROADCASTING_ID:
d, r = self.RxPacket(echo)
if r:
return id == d[3] and d[1] == (self.OPT_LOAD | 0x80) and len(d) == 5
else:
return True;
return False
def SAVE(self, id : int, echo = False) -> bool:
with self.__lock:
if self.TxPacket(self.OPT_SAVE, 0x0, B2Bs(id), echo)[1]:
if id != self.BROADCASTING_ID:
d, r = self.RxPacket(echo)
if r:
return id == d[3] and d[1] == (self.OPT_SAVE| 0x80) and len(d) == 5
else:
return True;
return False
def POSITION(self, id : int, pos : int, tim : int, reset = False, echo = False) -> int:
with self.__lock:
if pos >= -32000 and pos <= 32000 and tim >= 0 and tim <= 65535:
if self.TxPacket(self.OPT_POSITION, 0x80 if reset else 0x00, B2Bs(id) + W2Bs(pos) + W2Bs(tim), echo):
if id != self.BROADCASTING_ID:
dat, r = self.RxPacket(echo)
if r:
if dat[3] == id and len(dat) == 7:
return tuple(n[0] for n in iter_unpack('<h', dat[4:6]))[0]
else:
return None
else:
return None
return None
def POSITIONs(self, idposs : Union[list, tuple], tim : int, reset = False, echo = False) -> bool:
with self.__lock:
if tim >= 0 and tim <= 65535:
cnt = len(idposs)
if cnt > 0:
txd = bytes(0)
l = len(idposs[0])
for idpos in idposs:
if len(idpos) != 2:
return False
if idpos[0] < 0 or idpos[0] > 254 or len(idpos) != l or idpos[1] < -32000 or idpos[1] > 32000:
return False
txd += B2Bs(idpos[0]) + W2Bs(idpos[1])
txd += W2Bs(tim)
return self.TxPacket(self.OPT_POSITION, 0x80 if reset else 0x00, txd, echo)[1]
return False
def RESET(self, id : int, tim : int, echo = False) -> bool:
with self.__lock:
if ((id >= 0 and id <= 255) or id == self.BROADCASTING_ID) and tim >= 0 and tim <= 255:
if self.TxPacket(self.OPT_RESET, 0x0, B2Bs(id) + B2Bs(tim), echo)[1]:
return True
return False
def Ping(self, id : int, echo = False) -> bool:
if id >= 0 and id <= 254:
for i in range(3):
r = self.READ8(id, 0, 1, False, echo)
if r != None: return True
return False
##########################################################
# test code
##########################################################
if __name__ == "__main__":
import time
from pyB3M import *
try:
b3m = B3MProtocol('/dev/ttyAMA0', 1000000, 1)
except:
pass
else:
ID = 0
ec = True
# r/w test
print('reset=',b3m.RESET(ID, 0, echo = ec))
time.sleep(0.5)
print('load=',b3m.LOAD(ID, echo = ec))
print('save=',b3m.SAVE(ID, echo = ec))
print('read 5~9=',b3m.READ(ID, 0, 5, echo = ec).hex(','))
print('read32 0=',b3m.READ32(ID, 0, echo = ec))
print('read16 5=',b3m.READ16(ID, 5, echo = ec))
print('read16 7=',b3m.READ16(ID, 7, echo = ec))
print('read16 0x50=',b3m.READ16(ID, 0x50, echo = ec))
# r/w pid gain
gain = list(b3m.READ16(ID, 0x5e, 3, echo = ec))
print('pid r=',gain)
print('pid w=',b3m.WRITE16(ID, 0x5e, gain, echo = ec))
print('pid r=',b3m.READ16(ID, 0x5e, 3, echo = ec))
# write (multi)
d = (B2Bs(ID)+W2Bs(gain), B2Bs(ID+1)+W2Bs(gain), B2Bs(ID+2)+W2Bs(gain), B2Bs(ID+3)+W2Bs(gain),)
print(d)
print('writes=',b3m.WRITEs(0x5e, d, echo = ec))
# motor ctrl
# set free
if b3m.WRITE8(ID, 0x28, 2, echo = ec):
# sel pos mode
if b3m.WRITE8(ID, 0x28, 2, echo = ec):
# sel profile
if b3m.WRITE8(ID, 0x29, 1, echo = ec):
# set normal
if b3m.WRITE8(ID, 0x28, 0, echo = ec):
# position
for ang in (0, 4500, 9000, 4500, 0, -4500, -9000, -4500, 0):
b3m.POSITION(ID, ang, 500)
s = time.time() + 1
while s > time.time():
r = b3m.READ(ID, 0x27, 48)
if r != None:
n = tuple(iter_unpack('<BHBBhhhhhhHHHhIHhhHHHHiB', r))[0]
print(f'READ(0x27~0x56)={ang:7d}{n[5]:7d}{n[8]:7d} {n[16]:5d}{n[17]:5d}{n[18]:7d}{n[19]:7d}', end='\r')
else:
print('READ(0x27~0x56)=xxxxx', end='\r')
else:
print('err:set normal')
else:
print('err:set profile')
else:
print('err:set pos mode')
else:
print('err:set free')
print()
# set free
b3m.WRITE8(ID, 0x28, 2)
del b3m
print('fin')
全検索
IDやボーレートが分からない場合の検索。kbhit要。
B3M側の設定と異なるボーレートで疎通確認を行うため、B3Mの内部エラーが誘発されるのと、B3Mがパケットを誤認して設定が変わってしまうなどのリスクがあるので、あまり使うべきではない。
#!/usr/bin/python3
#
# scan.py
import sys, kbhit, time
from pyB3M import B3MProtocol
kb = kbhit.KBHit()
try:
b3m = B3MProtocol(sys.argv[1] if len(sys.argv) == 2 else '/dev/ttyAMA0', 115200, 0.02)
for b in (115200, 1000000, 1250000, 1500000, 2000000, 3000000):
b3m.baudrate = b
for id in range(255):
if kb.kbhit():
k = kb.getch()
break
print(f' baud:{b:7} id:{id:3} ', end='find\n' if b3m.Ping(id) else '\r', flush=True)
time.sleep(0.5)
sys.stdout.write('\033[2K\033[1G')
except:
print(' ERR:There is some problem.')
ID変更
任意のIDへ変更。
#!/usr/bin/python3
#
# changeid.py
# Note that since processing begins after LOAD is performed,
# various parameters are the same as immediately after startup.
import os, sys
from pyB3M import B3MProtocol
baudlist = { 115200 : 0, 1000000 : 1, 1250000 : 2, 1500000 : 3, 2000000 : 4, 3000000 : 5 }
if len(sys.argv) == 4:
arg = sys.argv[1:]
dev = '/dev/ttyAMA0'
elif len(sys.argv) == 5:
arg = sys.argv[2:]
dev = sys.argv[1]
else:
arg = []
dev = ''
if not os.path.exists(dev):
dev = ''
if dev != '':
try:
baud = int(arg[0])
baudind = baudlist[baud]
previd = int(arg[1])
newid = int(arg[2])
b3m = B3MProtocol(dev, baud, timeout = 0.1)
except:
print(' ERR:There is some problem.')
else:
if newid >= 0 and newid <= 254 and previd >= 0 and previd <= 254:
# Spell to eliminate the habit of poor readout of B3M
b3m.Ping(previd)
# Directs LOAD to the previous ID regardless of its presence or absence.
b3m.LOAD(previd)
# Confirm that the ID to be changed does not exist
if not b3m.Ping(newid):
# I don't know if it's necessary, but I'll put it in a free state.
if b3m.WRITE8(previd, 0x28, 2):
# Write new ID (It is an error because the response is changed with the new ID.)
dum = b3m.WRITE8(previd, 0, newid)
if b3m.Ping(newid):
# SAVE
if b3m.SAVE(newid):
print(' OK')
else:
print(' NG -1')
else:
print(' NG -2')
else:
print(f' ERR: Device with ID:{previd} not found with BAUDRATE:{baud}')
else:
print(' ERR:Competing.')
else:
print(' ERR:There is some problem.')
else:
print(' usage: changeid [line] <baudrate> <prev id> <new id>')
ボーレート変更
任意のボーレートへ変更。
セオリー通りの手順なのだが、手持ちのB3Mでは保存されないことが頻出して嫌気がさしている。
#!/usr/bin/python3
#
# changebaud.py
# Note that since processing begins after LOAD is performed,
# various parameters are the same as immediately after startup.
import os, sys
from pyB3M import B3MProtocol
baudlist = { 115200 : 0, 1000000 : 1, 1250000 : 2, 1500000 : 3, 2000000 : 4, 3000000 : 5 }
if len(sys.argv) == 4:
arg = sys.argv[1:]
dev = '/dev/ttyAMA0'
elif len(sys.argv) == 5:
arg = sys.argv[2:]
dev = sys.argv[1]
else:
arg = []
dev = ''
if not os.path.exists(dev):
dev = ''
if dev != '':
try:
prevbaud = int(arg[0])
prevind = baudlist[prevbaud]
id = int(arg[1])
newbaud = int(arg[2])
newind = baudlist[newbaud]
b3m = B3MProtocol(dev, newbaud, timeout = 0.1)
except:
print(' ERR: There is some problem.')
else:
# Verify that no devices exist at the changed baud rate
if not b3m.Ping(id):
b3m.baudrate = prevbaud
# Spell to eliminate the habit of poor readout of B3M
b3m.Ping(id)
# Directs LOAD to the previous ID regardless of its presence or absence.
b3m.LOAD(id)
# I don't know if it's necessary, but I'll put it in a free state.
if b3m.WRITE8(id, 0x28, 2):
# Write new baudrate
dum = b3m.WRITE32(id, 1, newbaud)
if b3m.Ping(id):
# SAVE
if b3m.SAVE(id):
b3m.RESET(id, 0)
b3m.baudrate = newbaud
if b3m.Ping(id):
print(' OK')
else:
print(' NG -1')
else:
print(' NG -2')
else:
print(' NG -3')
else:
print(f' ERR: Device with ID:{id} not found with BAUDRATE:{prevbaud}')
else:
print(' ERR: Competing.')
else:
print(' usage: changebaud [line] <baudrate> <id> <new baudrate>')
最終更新: 2024-04-22 (月) 14:34:47 (JST) (12d)
Copyright© 1989,2024 BestTechnology Co.,Ltd.
