From a8d3e132e456887f4fe816f4a9e04e8a40b16975 Mon Sep 17 00:00:00 2001 From: Elian NEPPEL <70491689+hubble14567@users.noreply.github.com> Date: Sat, 26 Oct 2024 16:43:53 +0900 Subject: [PATCH 1/5] formattted --- .../multi_controller.py | 263 +++++++++++------- 1 file changed, 158 insertions(+), 105 deletions(-) diff --git a/src/motor_controller/motor_controller/python_package_include/multi_controller.py b/src/motor_controller/motor_controller/python_package_include/multi_controller.py index c04a3f1..23658c5 100644 --- a/src/motor_controller/motor_controller/python_package_include/multi_controller.py +++ b/src/motor_controller/motor_controller/python_package_include/multi_controller.py @@ -28,7 +28,7 @@ # ********* DYNAMIXEL Model definition ********* # ***** (Use only one definition at a time) ***** -MY_DXL = 'X_SERIES' # X330 (5.0 V recommended), X430, X540, 2X430 +MY_DXL = "X_SERIES" # X330 (5.0 V recommended), X430, X540, 2X430 # MY_DXL = 'MX_SERIES' # MX series with 2.0 firmware update. # MY_DXL = 'PRO_SERIES' # H54, H42, M54, M42, L54, L42 # MY_DXL = 'PRO_A_SERIES' # PRO series with (A) firmware update. @@ -37,39 +37,31 @@ # see https://emanual.robotis.com/docs/en/dxl/x/xm430-w350/#control-table-of-ram-area or equivalent for you motor motor_address_table = { - 'X_SERIES': { + "X_SERIES": { "ADDR_TORQUE_ENABLE": 64, - "ADDR_LED_RED": 65, "LEN_LED_RED": 1, - "ADDR_GOAL_POSITION": 116, "LEN_GOAL_POSITION": 4, - "ADDR_PRESENT_POSITION": 132, "LEN_PRESENT_POSITION": 4, - "ADDR_MAX_VELOCITY": 112, "LEN_MAX_VELOCITY": 4, - "DXL_MINIMUM_POSITION_VALUE": 0, "DXL_MAXIMUM_POSITION_VALUE": 4095, - # "BAUDRATE": 57600, # "BAUDRATE": 1000000, "BAUDRATE": 4000000, - } - , - 'MX_SERIES': { + }, + "MX_SERIES": { "ADDR_TORQUE_ENABLE": 64, "ADDR_GOAL_POSITION": 116, "ADDR_PRESENT_POSITION": 132, "DXL_MINIMUM_POSITION_VALUE": 0, "DXL_MAXIMUM_POSITION_VALUE": 4095, - "BAUDRATE": 57600 - } - , - 'PRO_SERIES': { + "BAUDRATE": 57600, + }, + "PRO_SERIES": { "ADDR_TORQUE_ENABLE": 562, "ADDR_LED_RED": 563, "LEN_LED_RED": 1, @@ -80,9 +72,8 @@ "DXL_MINIMUM_POSITION_VALUE": -150000, "DXL_MAXIMUM_POSITION_VALUE": 150000, "BAUDRATE": 57600, - } - , - 'P_SERIES': { + }, + "P_SERIES": { "ADDR_TORQUE_ENABLE": 512, "ADDR_LED_RED": 513, "LEN_LED_RED": 1, @@ -93,9 +84,8 @@ "DXL_MINIMUM_POSITION_VALUE": -150000, "DXL_MAXIMUM_POSITION_VALUE": 150000, "BAUDRATE": 57600, - } - , - 'PRO_A_SERIES': { + }, + "PRO_A_SERIES": { "ADDR_TORQUE_ENABLE": 512, "ADDR_LED_RED": 513, "LEN_LED_RED": 1, @@ -106,17 +96,15 @@ "DXL_MINIMUM_POSITION_VALUE": -150000, "DXL_MAXIMUM_POSITION_VALUE": 150000, "BAUDRATE": 57600, - } - , - 'XL320': { + }, + "XL320": { "ADDR_TORQUE_ENABLE": 24, "ADDR_GOAL_POSITION": 30, "ADDR_PRESENT_POSITION": 37, "DXL_MINIMUM_POSITION_VALUE": 0, "DXL_MAXIMUM_POSITION_VALUE": 1023, - "BAUDRATE": 1000000 - } - , + "BAUDRATE": 1000000, + }, } @@ -125,9 +113,16 @@ class Motor: represents and handles a single motor """ - def __init__(self, motor_data, packetHandler, portHandler, groupBulkRead, groupBulkWrite, - deviceName='/dev/ttyUSB0', - motor_series="X_SERIES"): + def __init__( + self, + motor_data, + packetHandler, + portHandler, + groupBulkRead, + groupBulkWrite, + deviceName="/dev/ttyUSB0", + motor_series="X_SERIES", + ): self.id = motor_data[0] self.model = motor_data[1] self.firmware = motor_data[2] @@ -151,7 +146,9 @@ def rad2raw(self, radiant: float) -> int: :param radiant: angle in radiant :return: corresponding raw value for the motor """ - radiant += np.pi # 0 is everything pointing sraight, positive and negative angles should be used + radiant += ( + np.pi + ) # 0 is everything pointing sraight, positive and negative angles should be used raw = self.minraw + (self.maxraw - self.minraw) * (radiant / (2 * np.pi)) return int(np.clip(raw, self.minraw, self.maxraw)) @@ -165,13 +162,21 @@ def check_motor_alive(self, trial: int = 2) -> bool: :param trial: number of time the motors has been pinged :return: True if motor is alive, else False """ - dxl_model_number, dxl_comm_result, dxl_error = self.packetHandler.ping(self.portHandler, self.id) + dxl_model_number, dxl_comm_result, dxl_error = self.packetHandler.ping( + self.portHandler, self.id + ) if dxl_comm_result == COMM_SUCCESS: - print(f"after {trial} attempts ping successful, Motor {self.id:03d} Alive :)") + print( + f"after {trial} attempts ping successful, " + f"Motor {self.id:03d} Alive :)" + ) self.alive = True return True elif trial <= 1: - print(f"Motor {self.id} is dead {self.packetHandler.getRxPacketError(dxl_error)}") + print( + f"Motor {self.id} is dead " + f"{self.packetHandler.getRxPacketError(dxl_error)}" + ) self.alive = False return False else: @@ -182,10 +187,9 @@ def enable(self) -> None: Enables torque on the motor (the motor will have power) :return: """ - dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(self.portHandler, - self.id, - self.addr_table["ADDR_TORQUE_ENABLE"], - 1) + dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( + self.portHandler, self.id, self.addr_table["ADDR_TORQUE_ENABLE"], 1 + ) if dxl_comm_result != COMM_SUCCESS: print("%s" % self.packetHandler.getTxRxResult(dxl_comm_result)) self.check_motor_alive() @@ -200,10 +204,9 @@ def disable(self) -> None: Disable torque on the motor (the motor will power down) :return: """ - dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(self.portHandler, - self.id, - self.addr_table["ADDR_TORQUE_ENABLE"], - 0) + dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( + self.portHandler, self.id, self.addr_table["ADDR_TORQUE_ENABLE"], 0 + ) if dxl_comm_result != COMM_SUCCESS: print("%s" % self.packetHandler.getTxRxResult(dxl_comm_result)) self.check_motor_alive() @@ -218,9 +221,11 @@ def subscribe_position(self) -> None: The position of the motor will be requested on the bulk read :return: """ - dxl_addparam_result = self.groupBulkRead.addParam(self.id, - self.addr_table["ADDR_PRESENT_POSITION"], - self.addr_table["LEN_PRESENT_POSITION"]) + dxl_addparam_result = self.groupBulkRead.addParam( + self.id, + self.addr_table["ADDR_PRESENT_POSITION"], + self.addr_table["LEN_PRESENT_POSITION"], + ) if dxl_addparam_result != True: print(f"[ID:{self.id:03d}] subscribe_position failed\n{dxl_addparam_result}") @@ -231,9 +236,11 @@ def position_available(self) -> bool: The position has been returned and the data is available :return: """ - result_available = self.groupBulkRead.isAvailable(self.id, - self.addr_table["ADDR_PRESENT_POSITION"], - self.addr_table["LEN_PRESENT_POSITION"]) + result_available = self.groupBulkRead.isAvailable( + self.id, + self.addr_table["ADDR_PRESENT_POSITION"], + self.addr_table["LEN_PRESENT_POSITION"], + ) if not result_available: print(f"[ID:{self.id:03d}] position_available failed\n{result_available}") self.check_motor_alive() @@ -244,9 +251,11 @@ def get_position(self) -> float: Reads the position data received from the bulk read :return: """ - motor_raw_data = self.groupBulkRead.getData(self.id, - self.addr_table["ADDR_PRESENT_POSITION"], - self.addr_table["LEN_PRESENT_POSITION"]) + motor_raw_data = self.groupBulkRead.getData( + self.id, + self.addr_table["ADDR_PRESENT_POSITION"], + self.addr_table["LEN_PRESENT_POSITION"], + ) if motor_raw_data == 0: # error when getting the data # this 0 comes from the sdk, I don't know if the motor can ever return 0 as a valid position return np.nan @@ -261,15 +270,19 @@ def write_position(self, angle: float) -> None: :return: """ raw = self.rad2raw(angle) - param_goal_position = [DXL_LOBYTE(DXL_LOWORD(raw)), - DXL_HIBYTE(DXL_LOWORD(raw)), - DXL_LOBYTE(DXL_HIWORD(raw)), - DXL_HIBYTE(DXL_HIWORD(raw))] - - dxl_addparam_result = self.groupBulkWrite.addParam(self.id, - self.addr_table["ADDR_GOAL_POSITION"], - self.addr_table["LEN_GOAL_POSITION"], - param_goal_position) + param_goal_position = [ + DXL_LOBYTE(DXL_LOWORD(raw)), + DXL_HIBYTE(DXL_LOWORD(raw)), + DXL_LOBYTE(DXL_HIWORD(raw)), + DXL_HIBYTE(DXL_HIWORD(raw)), + ] + + dxl_addparam_result = self.groupBulkWrite.addParam( + self.id, + self.addr_table["ADDR_GOAL_POSITION"], + self.addr_table["LEN_GOAL_POSITION"], + param_goal_position, + ) if dxl_addparam_result != True: print("[ID:%03d] write_position failed" % self.id) self.check_motor_alive() @@ -286,15 +299,19 @@ def write_max_speed(self, speed_rads) -> None: raw = np.clip(int(np.floor(scaled)), self.minraw, self.maxraw) # print(raw) - param_goal_position = [DXL_LOBYTE(DXL_LOWORD(raw)), - DXL_HIBYTE(DXL_LOWORD(raw)), - DXL_LOBYTE(DXL_HIWORD(raw)), - DXL_HIBYTE(DXL_HIWORD(raw))] - - dxl_addparam_result = self.groupBulkWrite.addParam(self.id, - self.addr_table["ADDR_MAX_VELOCITY"], - self.addr_table["LEN_MAX_VELOCITY"], - param_goal_position) + param_goal_position = [ + DXL_LOBYTE(DXL_LOWORD(raw)), + DXL_HIBYTE(DXL_LOWORD(raw)), + DXL_LOBYTE(DXL_HIWORD(raw)), + DXL_HIBYTE(DXL_HIWORD(raw)), + ] + + dxl_addparam_result = self.groupBulkWrite.addParam( + self.id, + self.addr_table["ADDR_MAX_VELOCITY"], + self.addr_table["LEN_MAX_VELOCITY"], + param_goal_position, + ) if dxl_addparam_result != True: print("[ID:%03d] write_max_speed failed" % self.id) self.check_motor_alive() @@ -305,9 +322,15 @@ class MotorHandler: represent and handle several motors connected to the same port """ - def __init__(self, packetHandler, portHandler, groupBulkRead, groupBulkWrite, - deviceName='/dev/ttyUSB0', - motor_series="X_SERIES"): + def __init__( + self, + packetHandler, + portHandler, + groupBulkRead, + groupBulkWrite, + deviceName="/dev/ttyUSB0", + motor_series="X_SERIES", + ): self.motor_list = [] self.idrange = range(1, 17) @@ -351,15 +374,22 @@ def refresh_motors(self, idrange: list = range(1, 17)) -> list: if id in already_here_list: id_list.remove(id) - motor_data_array = search_for_motor(id_list, packetHandler=self.packetHandler, portHandler=self.portHandler) - - new_motors = [Motor(motor_data=motor_data_array[row, :], - packetHandler=self.packetHandler, - portHandler=self.portHandler, - groupBulkRead=self.groupBulkRead, - groupBulkWrite=self.groupBulkWrite, - deviceName=self.deviceName, - motor_series=self.motor_series) for row in range(motor_data_array.shape[0])] + motor_data_array = search_for_motor( + id_list, packetHandler=self.packetHandler, portHandler=self.portHandler + ) + + new_motors = [ + Motor( + motor_data=motor_data_array[row, :], + packetHandler=self.packetHandler, + portHandler=self.portHandler, + groupBulkRead=self.groupBulkRead, + groupBulkWrite=self.groupBulkWrite, + deviceName=self.deviceName, + motor_series=self.motor_series, + ) + for row in range(motor_data_array.shape[0]) + ] if new_motors: self.motor_list += new_motors @@ -405,7 +435,9 @@ def broadcast_max_speed(self, ang_speed: float): :param ang_speed: max speed allowed :return: True if success """ - return self.distibute_max_speeds(np.full(len(self.motor_list), ang_speed, dtype=float)) + return self.distibute_max_speeds( + np.full(len(self.motor_list), ang_speed, dtype=float) + ) def distibute_targets(self, angle_arr: np.ndarray): """ @@ -434,7 +466,13 @@ def all_positions_available(self) -> bool: Ture, if each alive motor has resonded to their data resquest :return: """ - return all([(my_motor.position_available()) for my_motor in self.motor_list if my_motor.alive]) + return all( + [ + (my_motor.position_available()) + for my_motor in self.motor_list + if my_motor.alive + ] + ) def get_angles(self) -> np.ndarray: """ @@ -447,7 +485,9 @@ def get_angles(self) -> np.ndarray: # if not comm_success: # try twice # break if comm_success: - return np.array([my_motor.get_position() for my_motor in self.motor_list], dtype=float) + return np.array( + [my_motor.get_position() for my_motor in self.motor_list], dtype=float + ) else: return np.full(len(self.motor_list), np.nan, dtype=float) @@ -460,8 +500,8 @@ def broadcast_target_on_time(self, angle: float, delta_time: float) -> bool: :return: True if success """ return self.to_target_same_time( - np.full(len(self.motor_list), angle, dtype=float), - delta_time) + np.full(len(self.motor_list), angle, dtype=float), delta_time + ) def to_target_same_time(self, angle_arr: np.ndarray, delta_time: float) -> bool: """ @@ -471,10 +511,14 @@ def to_target_same_time(self, angle_arr: np.ndarray, delta_time: float) -> bool: :param delta_time: time in sec to reach the target :return: True if success """ - delta_time_arr = np.full(shape=angle_arr.shape, fill_value=delta_time, dtype=float) + delta_time_arr = np.full( + shape=angle_arr.shape, fill_value=delta_time, dtype=float + ) return self.to_target_using_angle_time(angle_arr, delta_time_arr) - def to_target_using_angle_time(self, angle_arr: np.ndarray, delta_time_arr: np.ndarray) -> bool: + def to_target_using_angle_time( + self, angle_arr: np.ndarray, delta_time_arr: np.ndarray + ) -> bool: """ all motors will reach their target angle in the corresponding time in the delta_time_arr array see, self.get_motor_id_in_order to get the id corresponding to the index @@ -482,8 +526,9 @@ def to_target_using_angle_time(self, angle_arr: np.ndarray, delta_time_arr: np.n :param delta_time_arr: array of time in sec to reach the target :return: True if success """ - delta_time_arr_safe = np.clip(delta_time_arr, a_min=0.0001, - a_max=None) # avoid division by zero and negative values + delta_time_arr_safe = np.clip( + delta_time_arr, a_min=0.0001, a_max=None + ) # avoid division by zero and negative values angle_now = self.get_angles() speed = abs((angle_arr - angle_now) / delta_time_arr_safe) comm_result = self.distibute_max_speeds(speed) @@ -520,7 +565,9 @@ def request_update(self) -> bool: dxl_comm_result = self.groupBulkRead.txRxPacket() if dxl_comm_result != COMM_SUCCESS: - print(f"Request_update failed: {self.packetHandler.getTxRxResult(dxl_comm_result)}") + print( + f"Request_update failed: {self.packetHandler.getTxRxResult(dxl_comm_result)}" + ) motor_died = False for index in range(len(self.motor_list) - 1, -1, -1): my_motor = self.motor_list[index] @@ -590,7 +637,7 @@ def wave(x: float) -> float: try: # Use the actual port assigned to the U2D2. # ex) Windows: "COM*", Linux: "/dev/ttyUSB*", Mac: "/dev/tty.usbserial-*" - DEVICENAME = '/dev/ttyUSB0' + DEVICENAME = "/dev/ttyUSB0" # DYNAMIXEL Protocol Version (1.0 / 2.0) # https://emanual.robotis.com/docs/en/dxl/protocol2/ @@ -632,12 +679,14 @@ def wave(x: float) -> float: if 1: # continously do a sinusoid - controller = MotorHandler(packetHandler=packetHandler, - portHandler=portHandler, - groupBulkRead=groupBulkRead, - groupBulkWrite=groupBulkWrite, - deviceName='/dev/ttyUSB0', - motor_series="X_SERIES") + controller = MotorHandler( + packetHandler=packetHandler, + portHandler=portHandler, + groupBulkRead=groupBulkRead, + groupBulkWrite=groupBulkWrite, + deviceName="/dev/ttyUSB0", + motor_series="X_SERIES", + ) controller.refresh_motors() print(f"number of motors: {len(controller.motor_list)}\n") @@ -662,12 +711,14 @@ def wave(x: float) -> float: time.sleep(dtime - (after - before)) if 0: # compare set max speed and real speed - controller = MotorHandler(packetHandler=packetHandler, - portHandler=portHandler, - groupBulkRead=groupBulkRead, - groupBulkWrite=groupBulkWrite, - deviceName='/dev/ttyUSB0', - motor_series="X_SERIES") + controller = MotorHandler( + packetHandler=packetHandler, + portHandler=portHandler, + groupBulkRead=groupBulkRead, + groupBulkWrite=groupBulkWrite, + deviceName="/dev/ttyUSB0", + motor_series="X_SERIES", + ) controller.refresh_motors() print(f"number of motors: {len(controller.motor_list)}\n") @@ -697,7 +748,9 @@ def wave(x: float) -> float: real_speed = angle_arr / period - print(f"target speed = {speed}\nreal speed = {real_speed}\ncorrection factor = {speed / real_speed}") + print( + f"target speed = {speed}\nreal speed = {real_speed}\ncorrection factor = {speed / real_speed}" + ) except KeyboardInterrupt: pass # Disable Dynamixel Torque From 7f8e22f49fd5187ebcef4eeebc22a69e4722edf7 Mon Sep 17 00:00:00 2001 From: Elian NEPPEL <70491689+hubble14567@users.noreply.github.com> Date: Sat, 26 Oct 2024 16:44:39 +0900 Subject: [PATCH 2/5] formatted --- .../dynamixel_sdk/protocol2_packet_handler.py | 479 ++++++++++++++---- 1 file changed, 384 insertions(+), 95 deletions(-) diff --git a/src/motor_controller/motor_controller/python_package_include/dynamixel_sdk/protocol2_packet_handler.py b/src/motor_controller/motor_controller/python_package_include/dynamixel_sdk/protocol2_packet_handler.py index 6b2fd69..05c4575 100644 --- a/src/motor_controller/motor_controller/python_package_include/dynamixel_sdk/protocol2_packet_handler.py +++ b/src/motor_controller/motor_controller/python_package_include/dynamixel_sdk/protocol2_packet_handler.py @@ -100,50 +100,272 @@ def getRxPacketError(self, error): return "[RxPacketError] The data value exceeds the limit value!" elif not_alert_error == ERRNUM_ACCESS: - return "[RxPacketError] Writing or Reading is not available to target address!" + return ( + "[RxPacketError] Writing or Reading is not available to target address!" + ) else: return "[RxPacketError] Unknown error code!" def updateCRC(self, crc_accum, data_blk_ptr, data_blk_size): - crc_table = [0x0000, - 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, - 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, - 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, - 0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, - 0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9, - 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF, - 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5, - 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093, - 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, - 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, - 0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, - 0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, - 0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9, - 0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F, - 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176, - 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123, - 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, - 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, - 0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, - 0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, - 0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A, - 0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C, - 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5, - 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3, - 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, - 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, - 0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, - 0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, - 0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9, - 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC, - 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5, - 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243, - 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, - 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, - 0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, - 0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, - 0x820D, 0x8207, 0x0202] + crc_table = [ + 0x0000, + 0x8005, + 0x800F, + 0x000A, + 0x801B, + 0x001E, + 0x0014, + 0x8011, + 0x8033, + 0x0036, + 0x003C, + 0x8039, + 0x0028, + 0x802D, + 0x8027, + 0x0022, + 0x8063, + 0x0066, + 0x006C, + 0x8069, + 0x0078, + 0x807D, + 0x8077, + 0x0072, + 0x0050, + 0x8055, + 0x805F, + 0x005A, + 0x804B, + 0x004E, + 0x0044, + 0x8041, + 0x80C3, + 0x00C6, + 0x00CC, + 0x80C9, + 0x00D8, + 0x80DD, + 0x80D7, + 0x00D2, + 0x00F0, + 0x80F5, + 0x80FF, + 0x00FA, + 0x80EB, + 0x00EE, + 0x00E4, + 0x80E1, + 0x00A0, + 0x80A5, + 0x80AF, + 0x00AA, + 0x80BB, + 0x00BE, + 0x00B4, + 0x80B1, + 0x8093, + 0x0096, + 0x009C, + 0x8099, + 0x0088, + 0x808D, + 0x8087, + 0x0082, + 0x8183, + 0x0186, + 0x018C, + 0x8189, + 0x0198, + 0x819D, + 0x8197, + 0x0192, + 0x01B0, + 0x81B5, + 0x81BF, + 0x01BA, + 0x81AB, + 0x01AE, + 0x01A4, + 0x81A1, + 0x01E0, + 0x81E5, + 0x81EF, + 0x01EA, + 0x81FB, + 0x01FE, + 0x01F4, + 0x81F1, + 0x81D3, + 0x01D6, + 0x01DC, + 0x81D9, + 0x01C8, + 0x81CD, + 0x81C7, + 0x01C2, + 0x0140, + 0x8145, + 0x814F, + 0x014A, + 0x815B, + 0x015E, + 0x0154, + 0x8151, + 0x8173, + 0x0176, + 0x017C, + 0x8179, + 0x0168, + 0x816D, + 0x8167, + 0x0162, + 0x8123, + 0x0126, + 0x012C, + 0x8129, + 0x0138, + 0x813D, + 0x8137, + 0x0132, + 0x0110, + 0x8115, + 0x811F, + 0x011A, + 0x810B, + 0x010E, + 0x0104, + 0x8101, + 0x8303, + 0x0306, + 0x030C, + 0x8309, + 0x0318, + 0x831D, + 0x8317, + 0x0312, + 0x0330, + 0x8335, + 0x833F, + 0x033A, + 0x832B, + 0x032E, + 0x0324, + 0x8321, + 0x0360, + 0x8365, + 0x836F, + 0x036A, + 0x837B, + 0x037E, + 0x0374, + 0x8371, + 0x8353, + 0x0356, + 0x035C, + 0x8359, + 0x0348, + 0x834D, + 0x8347, + 0x0342, + 0x03C0, + 0x83C5, + 0x83CF, + 0x03CA, + 0x83DB, + 0x03DE, + 0x03D4, + 0x83D1, + 0x83F3, + 0x03F6, + 0x03FC, + 0x83F9, + 0x03E8, + 0x83ED, + 0x83E7, + 0x03E2, + 0x83A3, + 0x03A6, + 0x03AC, + 0x83A9, + 0x03B8, + 0x83BD, + 0x83B7, + 0x03B2, + 0x0390, + 0x8395, + 0x839F, + 0x039A, + 0x838B, + 0x038E, + 0x0384, + 0x8381, + 0x0280, + 0x8285, + 0x828F, + 0x028A, + 0x829B, + 0x029E, + 0x0294, + 0x8291, + 0x82B3, + 0x02B6, + 0x02BC, + 0x82B9, + 0x02A8, + 0x82AD, + 0x82A7, + 0x02A2, + 0x82E3, + 0x02E6, + 0x02EC, + 0x82E9, + 0x02F8, + 0x82FD, + 0x82F7, + 0x02F2, + 0x02D0, + 0x82D5, + 0x82DF, + 0x02DA, + 0x82CB, + 0x02CE, + 0x02C4, + 0x82C1, + 0x8243, + 0x0246, + 0x024C, + 0x8249, + 0x0258, + 0x825D, + 0x8257, + 0x0252, + 0x0270, + 0x8275, + 0x827F, + 0x027A, + 0x826B, + 0x026E, + 0x0264, + 0x8261, + 0x0220, + 0x8225, + 0x822F, + 0x022A, + 0x823B, + 0x023E, + 0x0234, + 0x8231, + 0x8213, + 0x0216, + 0x021C, + 0x8219, + 0x0208, + 0x820D, + 0x8207, + 0x0202, + ] for j in range(0, data_blk_size): i = ((crc_accum >> 8) ^ data_blk_ptr[j]) & 0xFF @@ -158,16 +380,20 @@ def addStuffing(self, packet): temp = [0] * TXPACKET_MAX_LEN # FF FF FD XX ID LEN_L LEN_H - temp[PKT_HEADER0: PKT_HEADER0 + PKT_LENGTH_H + 1] = packet[PKT_HEADER0: PKT_HEADER0 + PKT_LENGTH_H + 1] + temp[PKT_HEADER0 : PKT_HEADER0 + PKT_LENGTH_H + 1] = packet[ + PKT_HEADER0 : PKT_HEADER0 + PKT_LENGTH_H + 1 + ] index = PKT_INSTRUCTION for i in range(0, packet_length_in - 2): # except CRC temp[index] = packet[i + PKT_INSTRUCTION] index = index + 1 - if packet[i + PKT_INSTRUCTION] == 0xFD \ - and packet[i + PKT_INSTRUCTION - 1] == 0xFF \ - and packet[i + PKT_INSTRUCTION - 2] == 0xFF: + if ( + packet[i + PKT_INSTRUCTION] == 0xFD + and packet[i + PKT_INSTRUCTION - 1] == 0xFF + and packet[i + PKT_INSTRUCTION - 2] == 0xFF + ): # FF FF FD temp[index] = 0xFD index = index + 1 @@ -180,7 +406,7 @@ def addStuffing(self, packet): if packet_length_in != packet_length_out: packet = [0] * index - packet[0: index] = temp[0: index] + packet[0:index] = temp[0:index] packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out) packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out) @@ -193,8 +419,12 @@ def removeStuffing(self, packet): index = PKT_INSTRUCTION for i in range(0, (packet_length_in - 2)): # except CRC - if (packet[i + PKT_INSTRUCTION] == 0xFD) and (packet[i + PKT_INSTRUCTION + 1] == 0xFD) and ( - packet[i + PKT_INSTRUCTION - 1] == 0xFF) and (packet[i + PKT_INSTRUCTION - 2] == 0xFF): + if ( + (packet[i + PKT_INSTRUCTION] == 0xFD) + and (packet[i + PKT_INSTRUCTION + 1] == 0xFD) + and (packet[i + PKT_INSTRUCTION - 1] == 0xFF) + and (packet[i + PKT_INSTRUCTION - 2] == 0xFF) + ): # FF FF FD FD packet_length_out = packet_length_out - 1 else: @@ -218,7 +448,9 @@ def txPacket(self, port, txpacket): self.addStuffing(txpacket) # check max packet length - total_packet_length = DXL_MAKEWORD(txpacket[PKT_LENGTH_L], txpacket[PKT_LENGTH_H]) + 7 + total_packet_length = ( + DXL_MAKEWORD(txpacket[PKT_LENGTH_L], txpacket[PKT_LENGTH_H]) + 7 + ) # 7: HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H if total_packet_length > TXPACKET_MAX_LEN: @@ -259,21 +491,39 @@ def rxPacket(self, port): if rx_length >= wait_length: # find packet header for idx in range(0, (rx_length - 3)): - if (rxpacket[idx] == 0xFF) and (rxpacket[idx + 1] == 0xFF) and (rxpacket[idx + 2] == 0xFD) and ( - rxpacket[idx + 3] != 0xFD): + if ( + (rxpacket[idx] == 0xFF) + and (rxpacket[idx + 1] == 0xFF) + and (rxpacket[idx + 2] == 0xFD) + and (rxpacket[idx + 3] != 0xFD) + ): break if idx == 0: - if (rxpacket[PKT_RESERVED] != 0x00) or (rxpacket[PKT_ID] > 0xFC) or ( - DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) > RXPACKET_MAX_LEN) or ( - rxpacket[PKT_INSTRUCTION] != 0x55): + if ( + (rxpacket[PKT_RESERVED] != 0x00) + or (rxpacket[PKT_ID] > 0xFC) + or ( + DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + > RXPACKET_MAX_LEN + ) + or (rxpacket[PKT_INSTRUCTION] != 0x55) + ): # remove the first byte in the packet del rxpacket[0] rx_length -= 1 continue - if wait_length != (DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1): - wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1 + if wait_length != ( + DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + + PKT_LENGTH_H + + 1 + ): + wait_length = ( + DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + + PKT_LENGTH_H + + 1 + ) continue if rx_length < wait_length: @@ -286,7 +536,9 @@ def rxPacket(self, port): else: continue - crc = DXL_MAKEWORD(rxpacket[wait_length - 2], rxpacket[wait_length - 1]) + crc = DXL_MAKEWORD( + rxpacket[wait_length - 2], rxpacket[wait_length - 1] + ) if self.updateCRC(0, rxpacket, wait_length - 2) == crc: result = COMM_SUCCESS @@ -296,7 +548,7 @@ def rxPacket(self, port): else: # remove unnecessary packets - del rxpacket[0: idx] + del rxpacket[0:idx] rx_length -= idx else: @@ -325,7 +577,10 @@ def txRxPacket(self, port, txpacket): return rxpacket, result, error # (Instruction == BulkRead or SyncRead) == this function is not available. - if txpacket[PKT_INSTRUCTION] == INST_BULK_READ or txpacket[PKT_INSTRUCTION] == INST_SYNC_READ: + if ( + txpacket[PKT_INSTRUCTION] == INST_BULK_READ + or txpacket[PKT_INSTRUCTION] == INST_SYNC_READ + ): result = COMM_NOT_AVAILABLE # (ID == Broadcast ID) == no need to wait for status packet or not available. @@ -336,7 +591,10 @@ def txRxPacket(self, port, txpacket): # set packet timeout if txpacket[PKT_INSTRUCTION] == INST_READ: - port.setPacketTimeout(DXL_MAKEWORD(txpacket[PKT_PARAMETER0 + 2], txpacket[PKT_PARAMETER0 + 3]) + 11) + port.setPacketTimeout( + DXL_MAKEWORD(txpacket[PKT_PARAMETER0 + 2], txpacket[PKT_PARAMETER0 + 3]) + + 11 + ) else: port.setPacketTimeout(11) # HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H @@ -368,7 +626,9 @@ def ping(self, port, dxl_id): rxpacket, result, error = self.txRxPacket(port, txpacket) if result == COMM_SUCCESS: - model_number = DXL_MAKEWORD(rxpacket[PKT_PARAMETER0 + 1], rxpacket[PKT_PARAMETER0 + 2]) + model_number = DXL_MAKEWORD( + rxpacket[PKT_PARAMETER0 + 1], rxpacket[PKT_PARAMETER0 + 2] + ) return model_number, result, error @@ -383,7 +643,7 @@ def broadcastPing(self, port): txpacket = [0] * 10 rxpacket = [] - tx_time_per_byte = (1000.0 / port.getBaudRate()) *10.0; + tx_time_per_byte = (1000.0 / port.getBaudRate()) * 10.0 txpacket[PKT_ID] = BROADCAST_ID txpacket[PKT_LENGTH_L] = 3 @@ -396,8 +656,10 @@ def broadcastPing(self, port): return data_list, result # set rx timeout - #port.setPacketTimeout(wait_length * 1) - port.setPacketTimeoutMillis((wait_length * tx_time_per_byte) + (3.0 * MAX_ID) + 16.0); + # port.setPacketTimeout(wait_length * 1) + port.setPacketTimeoutMillis( + (wait_length * tx_time_per_byte) + (3.0 * MAX_ID) + 16.0 + ) while True: rxpacket += port.readPort(wait_length - rx_length) @@ -417,21 +679,30 @@ def broadcastPing(self, port): # find packet header for idx in range(0, rx_length - 2): - if rxpacket[idx] == 0xFF and rxpacket[idx + 1] == 0xFF and rxpacket[idx + 2] == 0xFD: + if ( + rxpacket[idx] == 0xFF + and rxpacket[idx + 1] == 0xFF + and rxpacket[idx + 2] == 0xFD + ): break if idx == 0: # found at the beginning of the packet # verify CRC16 - crc = DXL_MAKEWORD(rxpacket[STATUS_LENGTH - 2], rxpacket[STATUS_LENGTH - 1]) + crc = DXL_MAKEWORD( + rxpacket[STATUS_LENGTH - 2], rxpacket[STATUS_LENGTH - 1] + ) if self.updateCRC(0, rxpacket, STATUS_LENGTH - 2) == crc: result = COMM_SUCCESS data_list[rxpacket[PKT_ID]] = [ - DXL_MAKEWORD(rxpacket[PKT_PARAMETER0 + 1], rxpacket[PKT_PARAMETER0 + 2]), - rxpacket[PKT_PARAMETER0 + 3]] + DXL_MAKEWORD( + rxpacket[PKT_PARAMETER0 + 1], rxpacket[PKT_PARAMETER0 + 2] + ), + rxpacket[PKT_PARAMETER0 + 3], + ] - del rxpacket[0: STATUS_LENGTH] + del rxpacket[0:STATUS_LENGTH] rx_length = rx_length - STATUS_LENGTH if rx_length == 0: @@ -441,12 +712,12 @@ def broadcastPing(self, port): result = COMM_RX_CORRUPT # remove header (0xFF 0xFF 0xFD) - del rxpacket[0: 3] + del rxpacket[0:3] rx_length = rx_length - 3 else: # remove unnecessary packets - del rxpacket[0: idx] + del rxpacket[0:idx] rx_length = rx_length - idx # FIXME: unreachable code @@ -541,7 +812,7 @@ def readRx(self, port, dxl_id, length): if result == COMM_SUCCESS and rxpacket[PKT_ID] == dxl_id: error = rxpacket[PKT_ERROR] - data.extend(rxpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length]) + data.extend(rxpacket[PKT_PARAMETER0 + 1 : PKT_PARAMETER0 + 1 + length]) return data, result, error @@ -567,7 +838,7 @@ def readTxRx(self, port, dxl_id, address, length): if result == COMM_SUCCESS: error = rxpacket[PKT_ERROR] - data.extend(rxpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length]) + data.extend(rxpacket[PKT_PARAMETER0 + 1 : PKT_PARAMETER0 + 1 + length]) return data, result, error @@ -602,14 +873,20 @@ def read4ByteTx(self, port, dxl_id, address): def read4ByteRx(self, port, dxl_id): data, result, error = self.readRx(port, dxl_id, 4) - data_read = DXL_MAKEDWORD(DXL_MAKEWORD(data[0], data[1]), - DXL_MAKEWORD(data[2], data[3])) if (result == COMM_SUCCESS) else 0 + data_read = ( + DXL_MAKEDWORD(DXL_MAKEWORD(data[0], data[1]), DXL_MAKEWORD(data[2], data[3])) + if (result == COMM_SUCCESS) + else 0 + ) return data_read, result, error def read4ByteTxRx(self, port, dxl_id, address): data, result, error = self.readTxRx(port, dxl_id, address, 4) - data_read = DXL_MAKEDWORD(DXL_MAKEWORD(data[0], data[1]), - DXL_MAKEWORD(data[2], data[3])) if (result == COMM_SUCCESS) else 0 + data_read = ( + DXL_MAKEDWORD(DXL_MAKEWORD(data[0], data[1]), DXL_MAKEWORD(data[2], data[3])) + if (result == COMM_SUCCESS) + else 0 + ) return data_read, result, error def writeTxOnly(self, port, dxl_id, address, length, data): @@ -622,7 +899,7 @@ def writeTxOnly(self, port, dxl_id, address, length, data): txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(address) txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(address) - txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + length] = data[0: length] + txpacket[PKT_PARAMETER0 + 2 : PKT_PARAMETER0 + 2 + length] = data[0:length] result = self.txPacket(port, txpacket) port.is_using = False @@ -639,7 +916,7 @@ def writeTxRx(self, port, dxl_id, address, length, data): txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(address) txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(address) - txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + length] = data[0: length] + txpacket[PKT_PARAMETER0 + 2 : PKT_PARAMETER0 + 2 + length] = data[0:length] rxpacket, result, error = self.txRxPacket(port, txpacket) return result, error @@ -661,17 +938,21 @@ def write2ByteTxRx(self, port, dxl_id, address, data): return self.writeTxRx(port, dxl_id, address, 2, data_write) def write4ByteTxOnly(self, port, dxl_id, address, data): - data_write = [DXL_LOBYTE(DXL_LOWORD(data)), - DXL_HIBYTE(DXL_LOWORD(data)), - DXL_LOBYTE(DXL_HIWORD(data)), - DXL_HIBYTE(DXL_HIWORD(data))] + data_write = [ + DXL_LOBYTE(DXL_LOWORD(data)), + DXL_HIBYTE(DXL_LOWORD(data)), + DXL_LOBYTE(DXL_HIWORD(data)), + DXL_HIBYTE(DXL_HIWORD(data)), + ] return self.writeTxOnly(port, dxl_id, address, 4, data_write) def write4ByteTxRx(self, port, dxl_id, address, data): - data_write = [DXL_LOBYTE(DXL_LOWORD(data)), - DXL_HIBYTE(DXL_LOWORD(data)), - DXL_LOBYTE(DXL_HIWORD(data)), - DXL_HIBYTE(DXL_HIWORD(data))] + data_write = [ + DXL_LOBYTE(DXL_LOWORD(data)), + DXL_HIBYTE(DXL_LOWORD(data)), + DXL_LOBYTE(DXL_HIWORD(data)), + DXL_HIBYTE(DXL_HIWORD(data)), + ] return self.writeTxRx(port, dxl_id, address, 4, data_write) def regWriteTxOnly(self, port, dxl_id, address, length, data): @@ -684,7 +965,7 @@ def regWriteTxOnly(self, port, dxl_id, address, length, data): txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(address) txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(address) - txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + length] = data[0: length] + txpacket[PKT_PARAMETER0 + 2 : PKT_PARAMETER0 + 2 + length] = data[0:length] result = self.txPacket(port, txpacket) port.is_using = False @@ -701,7 +982,7 @@ def regWriteTxRx(self, port, dxl_id, address, length, data): txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(address) txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(address) - txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + length] = data[0: length] + txpacket[PKT_PARAMETER0 + 2 : PKT_PARAMETER0 + 2 + length] = data[0:length] _, result, error = self.txRxPacket(port, txpacket) @@ -713,16 +994,20 @@ def syncReadTx(self, port, start_address, data_length, param, param_length): txpacket[PKT_ID] = BROADCAST_ID txpacket[PKT_LENGTH_L] = DXL_LOBYTE( - param_length + 7) # 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + param_length + 7 + ) # 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE( - param_length + 7) # 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + param_length + 7 + ) # 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H txpacket[PKT_INSTRUCTION] = INST_SYNC_READ txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(start_address) txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(start_address) txpacket[PKT_PARAMETER0 + 2] = DXL_LOBYTE(data_length) txpacket[PKT_PARAMETER0 + 3] = DXL_HIBYTE(data_length) - txpacket[PKT_PARAMETER0 + 4: PKT_PARAMETER0 + 4 + param_length] = param[0: param_length] + txpacket[PKT_PARAMETER0 + 4 : PKT_PARAMETER0 + 4 + param_length] = param[ + 0:param_length + ] result = self.txPacket(port, txpacket) if result == COMM_SUCCESS: @@ -736,16 +1021,20 @@ def syncWriteTxOnly(self, port, start_address, data_length, param, param_length) txpacket[PKT_ID] = BROADCAST_ID txpacket[PKT_LENGTH_L] = DXL_LOBYTE( - param_length + 7) # 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + param_length + 7 + ) # 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE( - param_length + 7) # 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + param_length + 7 + ) # 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(start_address) txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(start_address) txpacket[PKT_PARAMETER0 + 2] = DXL_LOBYTE(data_length) txpacket[PKT_PARAMETER0 + 3] = DXL_HIBYTE(data_length) - txpacket[PKT_PARAMETER0 + 4: PKT_PARAMETER0 + 4 + param_length] = param[0: param_length] + txpacket[PKT_PARAMETER0 + 4 : PKT_PARAMETER0 + 4 + param_length] = param[ + 0:param_length + ] _, result, _ = self.txRxPacket(port, txpacket) @@ -760,7 +1049,7 @@ def bulkReadTx(self, port, param, param_length): txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3) # 3: INST CRC16_L CRC16_H txpacket[PKT_INSTRUCTION] = INST_BULK_READ - txpacket[PKT_PARAMETER0: PKT_PARAMETER0 + param_length] = param[0: param_length] + txpacket[PKT_PARAMETER0 : PKT_PARAMETER0 + param_length] = param[0:param_length] result = self.txPacket(port, txpacket) if result == COMM_SUCCESS: @@ -782,7 +1071,7 @@ def bulkWriteTxOnly(self, port, param, param_length): txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3) # 3: INST CRC16_L CRC16_H txpacket[PKT_INSTRUCTION] = INST_BULK_WRITE - txpacket[PKT_PARAMETER0: PKT_PARAMETER0 + param_length] = param[0: param_length] + txpacket[PKT_PARAMETER0 : PKT_PARAMETER0 + param_length] = param[0:param_length] _, result, _ = self.txRxPacket(port, txpacket) From 75f6d5d0fee83944ddfad9711a780d4a41862bc2 Mon Sep 17 00:00:00 2001 From: Elian NEPPEL <70491689+hubble14567@users.noreply.github.com> Date: Sat, 26 Oct 2024 17:05:40 +0900 Subject: [PATCH 3/5] reboot methode and type hinting --- .../multi_controller.py | 59 ++++++++++++++----- 1 file changed, 44 insertions(+), 15 deletions(-) diff --git a/src/motor_controller/motor_controller/python_package_include/multi_controller.py b/src/motor_controller/motor_controller/python_package_include/multi_controller.py index 23658c5..19156b7 100644 --- a/src/motor_controller/motor_controller/python_package_include/multi_controller.py +++ b/src/motor_controller/motor_controller/python_package_include/multi_controller.py @@ -21,8 +21,10 @@ import os import time +from typing import List, Sequence, Union import numpy as np +from numpy.typing import NDArray from dynamixel_sdk import * # Uses Dynamixel SDK library @@ -115,13 +117,13 @@ class Motor: def __init__( self, - motor_data, - packetHandler, - portHandler, - groupBulkRead, - groupBulkWrite, - deviceName="/dev/ttyUSB0", - motor_series="X_SERIES", + motor_data: Union[Sequence, NDArray], + packetHandler: Protocol2PacketHandler, + portHandler: PortHandler, + groupBulkRead: GroupBulkRead, + groupBulkWrite: GroupBulkWrite, + deviceName: str = "/dev/ttyUSB0", + motor_series: str = "X_SERIES", ): self.id = motor_data[0] self.model = motor_data[1] @@ -182,6 +184,25 @@ def check_motor_alive(self, trial: int = 2) -> bool: else: return self.check_motor_alive(trial - 1) + def reboot(self) -> bool: + """sends reboot command to motor. This clears emergencies stop. + It then pings the motor to check if it's alive. + + Returns: + True if reboot communication succesfull and motor alive + """ + dxl_comm_result, dxl_error = self.packetHandler.reboot(self.portHandler, self.id) + if dxl_comm_result == COMM_SUCCESS: + print(f"Motor {self.id:03d} rebooted :)") + return self.check_motor_alive(trial=5) + else: + print( + f"Motor {self.id} FAILED reboot:" + f"{self.packetHandler.getRxPacketError(dxl_error)}" + ) + self.check_motor_alive() + return False + def enable(self) -> None: """ Enables torque on the motor (the motor will have power) @@ -324,14 +345,14 @@ class MotorHandler: def __init__( self, - packetHandler, - portHandler, - groupBulkRead, - groupBulkWrite, - deviceName="/dev/ttyUSB0", - motor_series="X_SERIES", + packetHandler: Protocol2PacketHandler, + portHandler: PortHandler, + groupBulkRead: GroupBulkRead, + groupBulkWrite: GroupBulkWrite, + deviceName: str = "/dev/ttyUSB0", + motor_series: str = "X_SERIES", ): - self.motor_list = [] + self.motor_list: List[Motor] = [] self.idrange = range(1, 17) self.packetHandler = packetHandler @@ -360,7 +381,7 @@ def delete_dead_motors(self) -> list: motor.subscribe_position() return motor_died - def refresh_motors(self, idrange: list = range(1, 17)) -> list: + def refresh_motors(self, idrange: Sequence = range(1, 17)) -> List: """ check if the motors with the provided ids are available if yes those motor are added to the list of motors managed by the handler @@ -405,6 +426,14 @@ def get_motor_id_in_order(self) -> list: """ return [motor.id for motor in self.motor_list] + def reboot_all(self) -> List[bool]: + """Reboots all motors currently handled. This clears emergencies and such. + + Returns: + list of if there was an error or not + """ + return [my_motor.reboot() for my_motor in self.motor_list] + def disable(self): """ Disables all motor From 47a36cad777e6e2b65fe18d47d6b01b31710b200 Mon Sep 17 00:00:00 2001 From: Elian NEPPEL <70491689+hubble14567@users.noreply.github.com> Date: Sat, 26 Oct 2024 17:36:47 +0900 Subject: [PATCH 4/5] srv created --- .../multi_controller.py | 6 +-- .../motor_controller/u2d2_dyna_controller.py | 42 +++++++++++++++---- 2 files changed, 36 insertions(+), 12 deletions(-) diff --git a/src/motor_controller/motor_controller/python_package_include/multi_controller.py b/src/motor_controller/motor_controller/python_package_include/multi_controller.py index 19156b7..ca7b573 100644 --- a/src/motor_controller/motor_controller/python_package_include/multi_controller.py +++ b/src/motor_controller/motor_controller/python_package_include/multi_controller.py @@ -125,9 +125,9 @@ def __init__( deviceName: str = "/dev/ttyUSB0", motor_series: str = "X_SERIES", ): - self.id = motor_data[0] - self.model = motor_data[1] - self.firmware = motor_data[2] + self.id: int = motor_data[0] + self.model: int = motor_data[1] + self.firmware: int = motor_data[2] self.packetHandler = packetHandler self.portHandler = portHandler diff --git a/src/motor_controller/motor_controller/u2d2_dyna_controller.py b/src/motor_controller/motor_controller/u2d2_dyna_controller.py index 2cc13d1..b22aa08 100644 --- a/src/motor_controller/motor_controller/u2d2_dyna_controller.py +++ b/src/motor_controller/motor_controller/u2d2_dyna_controller.py @@ -6,13 +6,14 @@ """ import time +from typing import Iterable, Iterator, List, Optional, Sequence, Tuple import numpy as np import traceback import rclpy from rclpy.node import Node from std_msgs.msg import Float64 -from std_srvs.srv import Empty +from std_srvs.srv import Empty, Trigger from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup from dyna_controller_messages.msg import AngleTime @@ -20,7 +21,7 @@ import python_package_include.multi_controller as my_controller import serial import termios -import time # Only use time when the ros2 clock is not usuable +import time # Only use time when the ros2 clock is not usuable def error_catcher(func): @@ -183,7 +184,7 @@ def __init__(self): self.PROTOCOL_VERSION: str = "2.00" # Will be defined when connecting - self.controller = None + self.controller: Optional[my_controller.MotorHandler] = None self.groupBulkWrite = None self.groupBulkRead = None self.packetHandler = None @@ -196,8 +197,11 @@ def __init__(self): ############ V Service V # \ / # # \/ # + self.rebootSRV = self.create_service( + Trigger, f"{self.PortAlias}_reboot_all", self.reboot_allSRVCBK + ) self.iAmAlive = self.create_service( - Empty, f"usb_{self.PortAlias}_alive", lambda: None + Empty, f"usb_{self.PortAlias}_alive", lambda q, r: None ) # /\ # # / \ # @@ -228,6 +232,25 @@ def __init__(self): # / \ # ############ ^ Timers ^ + @error_catcher + def reboot_allSRVCBK( + self, req: Trigger.Request, res: Trigger.Response + ) -> Trigger.Response: + if self.controller is None: + res.success = False + res.message = "Controller does not exist" + return res + + success_comm = self.controller.reboot_all() + res.success = all(success_comm) + if not res.success: + id_and_success: Iterable[Tuple[int, bool]] = zip( + map(lambda M: M.id, self.controller.motor_list), success_comm + ) + res.message = "Communication or reboot of a least one motor failed." + f"motor id and com success: {id_and_success}" + return res + @error_catcher def send_writen_angles(self): """ @@ -314,7 +337,6 @@ def search_motors(self, id_range: list): self.add_new_motor(motor_id) return motor_found - @error_catcher def add_new_motor(self, motor_id): """ Motor with the id will have its callback holder created @@ -327,7 +349,6 @@ def add_new_motor(self, motor_id): parent_node=self, ) - @error_catcher def delete_motor(self, motor_id): """ Destroys pub sub and associated cbk holder to the motor id @@ -411,8 +432,9 @@ def connect_and_setup(self, delete_controller=True): except serial.serialutil.SerialException as e: if e.errno == 2: self.get_logger().warning( - f"Port {self.PortAlias} [path: {self.DEVICENAME}, proto: {self.PROTOCOL_VERSION}]: Serial error 2 on opening, port/path may not exist" - , once = True) + f"Port {self.PortAlias} [path: {self.DEVICENAME}, proto: {self.PROTOCOL_VERSION}]: Serial error 2 on opening, port/path may not exist", + once=True, + ) else: raise e if opened: @@ -420,7 +442,8 @@ def connect_and_setup(self, delete_controller=True): break else: self.get_logger().warning( - f"Port {self.PortAlias}: failed to open. Silently retrying every {rate} s.", once=True + f"Port {self.PortAlias}: failed to open. Silently retrying every {rate} s.", + once=True, ) time.sleep(rate) @@ -451,6 +474,7 @@ def connect_and_setup(self, delete_controller=True): f"Port {self.PortAlias}: NO MOTOR, but setup successful :)" ) + def main(args=None): rclpy.init() From f4d0e924abb627b290129b474a33266f02fb662f Mon Sep 17 00:00:00 2001 From: Elian NEPPEL <70491689+hubble14567@users.noreply.github.com> Date: Sat, 26 Oct 2024 17:38:13 +0900 Subject: [PATCH 5/5] vim ignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 1f81bfd..33e994f 100644 --- a/.gitignore +++ b/.gitignore @@ -7,3 +7,4 @@ __pycache__ /log/ /install/ /build/ +Session.vim