diff --git a/dxl2/connection.py b/dxl2/connection.py index c6eac27..6a9d17f 100644 --- a/dxl2/connection.py +++ b/dxl2/connection.py @@ -1,12 +1,18 @@ import dynamixel_sdk as sdk +import glob +def get_ports(): + a = glob.glob("/dev/ttyUSB*") + glob.gob("/dev/ttyusbserial*") + return a[0] class Connection: - def __init__(self, port: str, baudrate: int = 1_000_000): + def __init__(self, port: str = None], baudrate: int = 1_000_000): self.port = port self.port_handler = sdk.PortHandler(port) self.baudrate = baudrate self.opened = False + if port == None: + self.ports = get_ports() def open_port(self) -> bool: ret = self.port_handler.openPort() diff --git a/dxl2/motor.py b/dxl2/motor.py index f81dcb4..d50bf2b 100644 --- a/dxl2/motor.py +++ b/dxl2/motor.py @@ -46,3 +46,4 @@ def read(self, instruction: Instruction) -> int: }[width](self.conn.port_handler, self.id, start) except KeyError: raise NotImplementedError(f"Register width not supported {width}") + diff --git a/dxl2/motor_chain.py b/dxl2/motor_chain.py index 1bf4e5f..aac1050 100644 --- a/dxl2/motor_chain.py +++ b/dxl2/motor_chain.py @@ -50,7 +50,12 @@ def write_many_values(self, instruction: Instruction, vals: Dict[int, int]) -> b self.conn.port_handler, self.packet_handler, pos, size ) for k, v in vals.items(): - if size == 2: + if size == 4: + dxl_addparam_result = groupSyncWrite.addParam( + k, util.create4ByteArray(v) + ) + + elif size == 2: dxl_addparam_result = groupSyncWrite.addParam( k, util.create2ByteArray(v) ) diff --git a/dxl2/util.py b/dxl2/util.py index ba4fe56..ca66bcc 100644 --- a/dxl2/util.py +++ b/dxl2/util.py @@ -21,3 +21,13 @@ def create2ByteArray(bin_value): def create1ByteArray(bin_value): byte_array = [DXL_LOBYTE(DXL_LOWORD(bin_value))] return byte_array + + +def create4ByteArray(bin_values): + byte_array = [ + DXL_LOBYTE(DXL_LOWORD(bin_value)), + DXL_HIBYTE(DXL_LOWORD(bin_value)), + DXL_LOBYTE(DXL_HIWORD(bin_value)), + DXL_HIBYTE(DXL_HIWORD(bin_value)), + ] + return byte_array