From eb63efa1a53027aa9df3f10cb1ecb21aafd7a97c Mon Sep 17 00:00:00 2001 From: vivekjagannath Date: Tue, 19 Mar 2019 18:42:05 +0530 Subject: [PATCH 1/5] default port selection added --- dxl2/connection.py | 4 +++- dxl2/motor.py | 1 + 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/dxl2/connection.py b/dxl2/connection.py index c6eac27..3d8f175 100644 --- a/dxl2/connection.py +++ b/dxl2/connection.py @@ -1,8 +1,10 @@ import dynamixel_sdk as sdk +import glob +a= glob.glob("/dev/ttyUSB*") + glob.glob("/dev/tty.usbserial*") class Connection: - def __init__(self, port: str, baudrate: int = 1_000_000): + def __init__(self, port: str = a[0], baudrate: int = 1_000_000): self.port = port self.port_handler = sdk.PortHandler(port) self.baudrate = baudrate 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}") + From 6378fb9d4a32ede7eac826a2371b5547f8f5fdcd Mon Sep 17 00:00:00 2001 From: vivekjagannath Date: Sun, 24 Mar 2019 17:05:33 +0530 Subject: [PATCH 2/5] added create4BtyeArray in util.py --- dxl2/util.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/dxl2/util.py b/dxl2/util.py index ba4fe56..4560adc 100644 --- a/dxl2/util.py +++ b/dxl2/util.py @@ -21,3 +21,8 @@ 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 From d6c7a190ce231395353592adb68b099836673bf2 Mon Sep 17 00:00:00 2001 From: vivekjagannath Date: Tue, 26 Mar 2019 18:00:46 +0530 Subject: [PATCH 3/5] added 4ByteArray func --- dxl2/motor_chain.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/dxl2/motor_chain.py b/dxl2/motor_chain.py index 1bf4e5f..34a2725 100644 --- a/dxl2/motor_chain.py +++ b/dxl2/motor_chain.py @@ -50,7 +50,10 @@ 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) ) From 48124d96da1dda7e3382dff04fab0bcd18641559 Mon Sep 17 00:00:00 2001 From: vivekjagannath Date: Tue, 26 Mar 2019 18:55:26 +0530 Subject: [PATCH 4/5] default port selection --- dxl2/connection.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/dxl2/connection.py b/dxl2/connection.py index 3d8f175..6a9d17f 100644 --- a/dxl2/connection.py +++ b/dxl2/connection.py @@ -1,14 +1,18 @@ import dynamixel_sdk as sdk import glob -a= glob.glob("/dev/ttyUSB*") + glob.glob("/dev/tty.usbserial*") +def get_ports(): + a = glob.glob("/dev/ttyUSB*") + glob.gob("/dev/ttyusbserial*") + return a[0] class Connection: - def __init__(self, port: str = a[0], 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() From d6562d9ec9ddfe1934ffe6b0ef00919d2f5dfd75 Mon Sep 17 00:00:00 2001 From: vivekjagannath Date: Tue, 2 Apr 2019 18:20:37 +0530 Subject: [PATCH 5/5] minor changes --- dxl2/motor_chain.py | 4 +++- dxl2/util.py | 7 ++++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/dxl2/motor_chain.py b/dxl2/motor_chain.py index 34a2725..aac1050 100644 --- a/dxl2/motor_chain.py +++ b/dxl2/motor_chain.py @@ -51,7 +51,9 @@ def write_many_values(self, instruction: Instruction, vals: Dict[int, int]) -> b ) for k, v in vals.items(): if size == 4: - dxl_addparam_result = groupSyncWrite.addParam( k, util.create4ByteArray(v)) + dxl_addparam_result = groupSyncWrite.addParam( + k, util.create4ByteArray(v) + ) elif size == 2: dxl_addparam_result = groupSyncWrite.addParam( diff --git a/dxl2/util.py b/dxl2/util.py index 4560adc..ca66bcc 100644 --- a/dxl2/util.py +++ b/dxl2/util.py @@ -24,5 +24,10 @@ def create1ByteArray(bin_value): 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))] + 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