From a9eec7daefba232cf39486b6aaf1899997893822 Mon Sep 17 00:00:00 2001 From: gumbald Date: Fri, 3 Apr 2020 13:59:19 +0100 Subject: [PATCH 1/3] Combined enums, fix int addition --- pydobot/dobot.py | 7 +-- pydobot/enums.py | 0 pydobot/enums/CommunicationProtocolIDs.py | 34 ------------ pydobot/enums/ControlValues.py | 15 ------ pydobot/enums/ptpMode.py | 64 ----------------------- pydobot/message.py | 2 +- 6 files changed, 3 insertions(+), 119 deletions(-) create mode 100644 pydobot/enums.py delete mode 100644 pydobot/enums/CommunicationProtocolIDs.py delete mode 100644 pydobot/enums/ControlValues.py delete mode 100644 pydobot/enums/ptpMode.py diff --git a/pydobot/dobot.py b/pydobot/dobot.py index 5fa8df7..f5f4839 100644 --- a/pydobot/dobot.py +++ b/pydobot/dobot.py @@ -4,11 +4,8 @@ import threading import warnings -from message import Message -from enums.PTPMode import PTPMode -from enums.CommunicationProtocolIDs import CommunicationProtocolIDs -from enums.ControlValues import ControlValues - +from .message import Message +from .enums import PTPMode, CommunicationProtocolIDs, ControlValues class Dobot: diff --git a/pydobot/enums.py b/pydobot/enums.py new file mode 100644 index 0000000..e69de29 diff --git a/pydobot/enums/CommunicationProtocolIDs.py b/pydobot/enums/CommunicationProtocolIDs.py deleted file mode 100644 index 2091841..0000000 --- a/pydobot/enums/CommunicationProtocolIDs.py +++ /dev/null @@ -1,34 +0,0 @@ -from enum import Enum - - -class CommunicationProtocolIDs(Enum): - - GET_SET_DEVICE_SN = 0 - GET_SET_DEVICE_NAME = 1 - GET_POSE = 10 - RESET_POSE = 11 - GET_ALARMS_STATE = 20 - CLEAR_ALL_ALARMS_STATE = 21 - SET_GET_HOME_PARAMS = 30 - SET_HOME_CMD = 31 - SET_GET_HHTTRIG_MODE = 40 - SET_GET_HHTTRIG_OUTPUT_ENABLED = 41 - GET_HHTTRIG_OUTPUT = 42 - SET_GET_ARM_ORIENTATION = 50 - SET_GET_END_EFFECTOR_PARAMS = 60 - SET_GET_END_EFFECTOR_LAZER = 61 - SET_GET_END_EFFECTOR_SUCTION_CUP = 62 - SET_GET_END_EFFECTOR_GRIPPER = 63 - SET_GET_JOG_JOINT_PARAMS = 70 - SET_GET_JOG_COORDINATE_PARAMS = 71 - SET_GET_JOG_COMMON_PARAMS = 72 - SET_GET_PTP_JOINT_PARAMS = 80 - SET_GET_PTP_COORDINATEP_ARAMS = 81 - SET_GET_PTP_JUMP_PARAMS = 82 - SET_GET_PTP_COMMON_PARAMS = 83 - SET_PTP_CMD = 84 - SET_CP_CMD = 91 - SET_QUEUED_CMD_START_EXEC = 240 - SET_QUEUED_CMD_STOP_EXEC = 241 - SET_QUEUED_CMD_CLEAR = 245 - GET_QUEUED_CMD_CURRENT_INDEX = 246 diff --git a/pydobot/enums/ControlValues.py b/pydobot/enums/ControlValues.py deleted file mode 100644 index b7a9eda..0000000 --- a/pydobot/enums/ControlValues.py +++ /dev/null @@ -1,15 +0,0 @@ -from enum import Enum - - -class ControlValues(Enum): - - ZERO = 0x00 - ONE = 0x01 - TWO = 0x02 - THREE = 0x03 - FOUR = 0x04 - FIVE = 0x05 - SIX = 0x06 - SEVEN = 0x07 - EIGHT = 0x08 - NINE = 0x09 diff --git a/pydobot/enums/ptpMode.py b/pydobot/enums/ptpMode.py deleted file mode 100644 index 09d7263..0000000 --- a/pydobot/enums/ptpMode.py +++ /dev/null @@ -1,64 +0,0 @@ -from enum import Enum - - -class PTPMode(Enum): - """ - 0. JUMP_XYZ, ] - Jump mode, - (x,y,z,r) - is the target point in Cartesian coordinate system - - 1. MOVJ_XYZ, - Joint movement, - (x,y,z,r) - is the target point in Cartesian coordinate system - - 2. MOVL_XYZ, - Linear movement, - (x,y,z,r) - is the target point in Cartesian coordinate system - - 3. JUMP_ANGLE, - Jump mode, (x,y,z,r) - is the target point in Jointcoordinate system - - 4. MOVJ_ANGLE, - Joint movement, - (x,y,z,r) - is the target point in Joint coordinate system - - 5. MOVL_ANGLE, - Linear movement, - (x,y,z,r) - is the target point in Joint coordinate system - - 6. MOVJ_INC, - Joint movement increment mode, - (x,y,z,r) - is the angle increment in Joint coordinate system - - 7. MOVL_INC, - Linear movement increment mode, - (x,y,z,r) - is the Cartesian coordinate increment in Joint coordinate system - - 8. MOVJ_XYZ_INC, - Joint movement increment mode, - (x,y,z,r) - is the Cartesian coordinate increment in Cartesian coordinate system - - 9. JUMP_MOVL_XYZ, - Jump movement, - (x,y,z,r) - is the Cartesian coordinate increment in Cartesian coordinate system - """ - JUMP_XYZ = 0x00 - MOVJ_XYZ = 0x01 - MOVL_XYZ = 0x02 - JUMP_ANGLE = 0x03 - MOVJ_ANGLE = 0x04 - MOVL_ANGLE = 0x05 - MOVJ_INC = 0x06 - MOVL_INC = 0x07 - MOVJ_XYZ_INC = 0x08 - JUMP_MOVL_XYZ = 0x09 diff --git a/pydobot/message.py b/pydobot/message.py index 915b238..04d9680 100644 --- a/pydobot/message.py +++ b/pydobot/message.py @@ -26,7 +26,7 @@ def __str__(self): def refresh(self): if self.checksum is None: - self.checksum = self.id + self.ctrl + self.checksum = int(self.id) + int(self.ctrl) for i in range(len(self.params)): if isinstance(self.params[i], int): self.checksum += self.params[i] From 6328b237b3fa50f939f8a29fe2f9335d583626b4 Mon Sep 17 00:00:00 2001 From: gumbald Date: Fri, 3 Apr 2020 16:24:51 +0100 Subject: [PATCH 2/3] Extended/corrected enum use --- pydobot/dobot.py | 4 +- pydobot/enums.py | 99 ++++++++++++++++++++++++++++++++++++++++++++++ pydobot/message.py | 16 ++++---- 3 files changed, 110 insertions(+), 9 deletions(-) diff --git a/pydobot/dobot.py b/pydobot/dobot.py index f5f4839..89e8119 100644 --- a/pydobot/dobot.py +++ b/pydobot/dobot.py @@ -5,7 +5,7 @@ import warnings from .message import Message -from .enums import PTPMode, CommunicationProtocolIDs, ControlValues +from .enums import ControlValues, PTPMode, CommunicationProtocolIDs class Dobot: @@ -221,7 +221,7 @@ def _set_ptp_cmd(self, x, y, z, r, mode, wait): msg.id = CommunicationProtocolIDs.SET_PTP_CMD msg.ctrl = ControlValues.THREE msg.params = bytearray([]) - msg.params.extend(bytearray([mode])) + msg.params.extend(bytearray([mode.value])) msg.params.extend(bytearray(struct.pack('f', x))) msg.params.extend(bytearray(struct.pack('f', y))) msg.params.extend(bytearray(struct.pack('f', z))) diff --git a/pydobot/enums.py b/pydobot/enums.py index e69de29..91add76 100644 --- a/pydobot/enums.py +++ b/pydobot/enums.py @@ -0,0 +1,99 @@ +from enum import Enum + +class CommunicationProtocolIDs(Enum): + + GET_SET_DEVICE_SN = 0 + GET_SET_DEVICE_NAME = 1 + GET_POSE = 10 + RESET_POSE = 11 + GET_ALARMS_STATE = 20 + CLEAR_ALL_ALARMS_STATE = 21 + SET_GET_HOME_PARAMS = 30 + SET_HOME_CMD = 31 + SET_GET_HHTTRIG_MODE = 40 + SET_GET_HHTTRIG_OUTPUT_ENABLED = 41 + GET_HHTTRIG_OUTPUT = 42 + SET_GET_ARM_ORIENTATION = 50 + SET_GET_END_EFFECTOR_PARAMS = 60 + SET_GET_END_EFFECTOR_LAZER = 61 + SET_GET_END_EFFECTOR_SUCTION_CUP = 62 + SET_GET_END_EFFECTOR_GRIPPER = 63 + SET_GET_JOG_JOINT_PARAMS = 70 + SET_GET_JOG_COORDINATE_PARAMS = 71 + SET_GET_JOG_COMMON_PARAMS = 72 + SET_GET_PTP_JOINT_PARAMS = 80 + SET_GET_PTP_COORDINATE_PARAMS = 81 + SET_GET_PTP_JUMP_PARAMS = 82 + SET_GET_PTP_COMMON_PARAMS = 83 + SET_PTP_CMD = 84 + SET_CP_CMD = 91 + SET_QUEUED_CMD_START_EXEC = 240 + SET_QUEUED_CMD_STOP_EXEC = 241 + SET_QUEUED_CMD_CLEAR = 245 + GET_QUEUED_CMD_CURRENT_INDEX = 246 + +class ControlValues(Enum): + + ZERO = 0x00 + ONE = 0x01 + TWO = 0x02 + THREE = 0x03 + FOUR = 0x04 + FIVE = 0x05 + SIX = 0x06 + SEVEN = 0x07 + EIGHT = 0x08 + NINE = 0x09 + +class PTPMode(Enum): + """ + 0. JUMP_XYZ, ] + Jump mode, + (x,y,z,r) + is the target point in Cartesian coordinate system + 1. MOVJ_XYZ, + Joint movement, + (x,y,z,r) + is the target point in Cartesian coordinate system + 2. MOVL_XYZ, + Linear movement, + (x,y,z,r) + is the target point in Cartesian coordinate system + 3. JUMP_ANGLE, + Jump mode, (x,y,z,r) + is the target point in Jointcoordinate system + 4. MOVJ_ANGLE, + Joint movement, + (x,y,z,r) + is the target point in Joint coordinate system + 5. MOVL_ANGLE, + Linear movement, + (x,y,z,r) + is the target point in Joint coordinate system + 6. MOVJ_INC, + Joint movement increment mode, + (x,y,z,r) + is the angle increment in Joint coordinate system + 7. MOVL_INC, + Linear movement increment mode, + (x,y,z,r) + is the Cartesian coordinate increment in Joint coordinate system + 8. MOVJ_XYZ_INC, + Joint movement increment mode, + (x,y,z,r) + is the Cartesian coordinate increment in Cartesian coordinate system + 9. JUMP_MOVL_XYZ, + Jump movement, + (x,y,z,r) + is the Cartesian coordinate increment in Cartesian coordinate system + """ + JUMP_XYZ = 0x00 + MOVJ_XYZ = 0x01 + MOVL_XYZ = 0x02 + JUMP_ANGLE = 0x03 + MOVJ_ANGLE = 0x04 + MOVL_ANGLE = 0x05 + MOVJ_INC = 0x06 + MOVL_INC = 0x07 + MOVJ_XYZ_INC = 0x08 + JUMP_MOVL_XYZ = 0x09 \ No newline at end of file diff --git a/pydobot/message.py b/pydobot/message.py index 04d9680..fa8f834 100644 --- a/pydobot/message.py +++ b/pydobot/message.py @@ -1,16 +1,18 @@ +from .enums import PTPMode, CommunicationProtocolIDs, ControlValues + class Message: def __init__(self, b=None): if b is None: self.header = bytes([0xAA, 0xAA]) self.len = 0x00 - self.ctrl = 0x00 + self.ctrl = ControlValues.ZERO self.params = bytes([]) self.checksum = None else: self.header = b[0:2] self.len = b[2] - self.id = b[3] - self.ctrl = b[4] + self.id = CommunicationProtocolIDs(b[3]) + self.ctrl = ControlValues(b[4]) self.params = b[5:-1] self.checksum = b[-1:][0] @@ -21,12 +23,12 @@ def __str__(self): self.refresh() hexHeader = " ".join("%02x" % b for b in self.header) hexParams = " ".join("%02x" % b for b in self.params) - ret = "%s:%d:%d:%d:%s:%s" % (hexHeader, self.len, self.id, self.ctrl, hexParams, self.checksum) + ret = "%s:%d:%d:%d:%s:%s" % (hexHeader, self.len, self.id.value, self.ctrl.value, hexParams, self.checksum) return ret.upper() def refresh(self): if self.checksum is None: - self.checksum = int(self.id) + int(self.ctrl) + self.checksum = self.id.value + self.ctrl.value for i in range(len(self.params)): if isinstance(self.params[i], int): self.checksum += self.params[i] @@ -40,9 +42,9 @@ def refresh(self): def bytes(self): self.refresh() if len(self.params) > 0: - command = bytearray([0xAA, 0xAA, self.len, self.id, self.ctrl]) + command = bytearray([0xAA, 0xAA, self.len, self.id.value, self.ctrl.value]) command.extend(self.params) command.append(self.checksum) else: - command = bytes([0xAA, 0xAA, self.len, self.id, self.ctrl, self.checksum]) + command = bytes([0xAA, 0xAA, self.len, self.id.value, self.ctrl.value, self.checksum]) return command From 477622dfda9a3ed0c2636fb925c7cee9d2b1bb1d Mon Sep 17 00:00:00 2001 From: gumbald Date: Mon, 6 Apr 2020 16:42:51 +0100 Subject: [PATCH 3/3] Added back home(), increase delay for read_message --- pydobot/dobot.py | 29 +++++++++++++++++++++-------- 1 file changed, 21 insertions(+), 8 deletions(-) diff --git a/pydobot/dobot.py b/pydobot/dobot.py index 89e8119..ccc7b4c 100644 --- a/pydobot/dobot.py +++ b/pydobot/dobot.py @@ -70,14 +70,15 @@ def _get_pose(self): (self.x, self.y, self.z, self.r, self.j1, self.j2, self.j3, self.j4)) return response - def _read_message(self): - time.sleep(0.1) - b = self.ser.read_all() - if len(b) > 0: - msg = Message(b) - if self.verbose: - print('pydobot: <<', msg) - return msg + def _read_message(self,retries=5): + for x in range(retries): + time.sleep(0.1) + b = self.ser.read_all() + if len(b) > 0: + msg = Message(b) + if self.verbose: + print('pydobot: <<', msg) + return msg return def _send_command(self, msg, wait=False): @@ -255,6 +256,15 @@ def _set_queued_cmd_stop_exec(self): msg.ctrl = ControlValues.ONE return self._send_command(msg) + """ + Home command + """ + def _set_home_cmd(self): + msg = Message() + msg.id = CommunicationProtocolIDs.SET_HOME_CMD + msg.ctrl = ControlValues.THREE + return self._send_command(msg, wait=True) + def close(self): self._on = False self.lock.acquire() @@ -291,3 +301,6 @@ def pose(self): j3 = struct.unpack_from('f', response.params, 24)[0] j4 = struct.unpack_from('f', response.params, 28)[0] return x, y, z, r, j1, j2, j3, j4 + + def home(self): + self._set_home_cmd()