diff --git a/pydobot/dobot.py b/pydobot/dobot.py index 5fa8df7..ccc7b4c 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 ControlValues, PTPMode, CommunicationProtocolIDs class Dobot: @@ -73,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): @@ -224,7 +222,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))) @@ -258,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() @@ -294,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() diff --git a/pydobot/enums/ptpMode.py b/pydobot/enums.py similarity index 56% rename from pydobot/enums/ptpMode.py rename to pydobot/enums.py index 09d7263..91add76 100644 --- a/pydobot/enums/ptpMode.py +++ b/pydobot/enums.py @@ -1,5 +1,49 @@ 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): """ @@ -7,46 +51,37 @@ class PTPMode(Enum): 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) @@ -61,4 +96,4 @@ class PTPMode(Enum): MOVJ_INC = 0x06 MOVL_INC = 0x07 MOVJ_XYZ_INC = 0x08 - JUMP_MOVL_XYZ = 0x09 + JUMP_MOVL_XYZ = 0x09 \ No newline at end of file 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/message.py b/pydobot/message.py index 915b238..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 = self.id + 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