Merge pull request #105 from elephantrobotics/mycobot_280_refactor

Mycobot 280 refactor

Author: wangWking

pymycobot/common.py

@@ -768,7 +768,7 @@ def write(self, command, method=None):
         self._serial_port.flush()
 
 
-def read(self, genre, method=None, command=None, _class=None, timeout=None):
+def read(self, genre, method=None, command=None, _class=None, timeout=None, real_command=None):
     datas = b""
     data_len = -1
     k = 0
@@ -800,9 +800,16 @@ def read(self, genre, method=None, command=None, _class=None, timeout=None):
         wait_time = 90
     elif genre == ProtocolCode.SET_SSID_PWD or genre == ProtocolCode.GET_SSID_PWD:
         wait_time = 0.05
+    if real_command:
+        if genre == ProtocolCode.SET_TOQUE_GRIPPER:
+            if real_command[6] == 13:
+                wait_time = 3
     data = b""
-
     if method is not None:
+        if real_command:
+            if genre == ProtocolCode.SET_TOQUE_GRIPPER:
+                if real_command[6] == 13:
+                    wait_time = 3
         if genre == 177:
             while True:
                 data = self.sock.recv(1024)

pymycobot/error.py

@@ -795,7 +795,7 @@ def calibration_parameters(**kwargs):
                     raise MyCobot320DataException(
                         "The range of 'gripper_id' in {} is 1 ~ 254, but the received value is {}".format(parameter,
                                                                                                           gripper_id))
-                if torque_value < 100 or torque_value > 300:
+                if torque_value < 0 or torque_value > 100:
                     raise MyCobot320DataException(
                         "The range of 'torque_value' in {} is 100 ~ 300, but the received value is {}".format(parameter,
                                                                                                               torque_value))

pymycobot/mycobot320.py

@@ -154,7 +154,7 @@ def _res(self, real_command, has_reply, genre):
             try_count = 0
             while try_count < 3:
                 self._write(self._flatten(real_command))
-                data = self._read(genre)
+                data = self._read(genre, real_command=real_command)
                 if data is not None and data != b'':
                     break
                 try_count += 1
@@ -165,6 +165,11 @@ def _res(self, real_command, has_reply, genre):
         res = self._process_received(data, genre)
         if res is None:
             return None
+        if genre == ProtocolCode.SET_TOQUE_GRIPPER:
+            if res == [0]:
+                self._write(self._flatten(real_command))
+                data = self._read(genre, real_command=real_command)
+                res = self._process_received(data, genre)
         if res is not None and isinstance(res, list) and len(res) == 1 and genre not in [ProtocolCode.GET_BASIC_VERSION,
                                                                                          ProtocolCode.GET_JOINT_MIN_ANGLE,
                                                                                          ProtocolCode.GET_JOINT_MAX_ANGLE,
@@ -201,6 +206,10 @@ def _res(self, real_command, has_reply, genre):
         ]:
             return self._process_single(res)
         elif genre in [ProtocolCode.GET_TOQUE_GRIPPER]:
+            if res[-1] == 255 and res[-2] == 255:
+                self._write(self._flatten(real_command))
+                data = self._read(genre)
+                res = self._process_received(data, genre)
             return self._process_high_low_bytes(res)
         elif genre in [ProtocolCode.GET_ANGLES]:
             return [self._int2angle(angle) for angle in res]

pymycobot/mycobot320socket.py

@@ -145,7 +145,7 @@ def _mesg(self, genre, *args, **kwargs):
                 try_count = 0
                 while try_count < 3:
                     self._write(self._flatten(real_command), "socket")
-                    data = self._read(genre, method='socket')
+                    data = self._read(genre, method='socket', real_command=real_command)
                     if data is not None and data != b'':
                         break
                     try_count += 1
@@ -156,7 +156,12 @@ def _mesg(self, genre, *args, **kwargs):
             res = self._process_received(data, genre)
             if res is None:
                 return None
-            elif res is not None and isinstance(res, list) and len(res) == 1 and genre not in [
+            if genre == ProtocolCode.SET_TOQUE_GRIPPER:
+                if res == [0]:
+                    self._write(self._flatten(real_command))
+                    data = self._read(genre, real_command=real_command)
+                    res = self._process_received(data, genre)
+            if res is not None and isinstance(res, list) and len(res) == 1 and genre not in [
                 ProtocolCode.GET_BASIC_VERSION,
                 ProtocolCode.GET_JOINT_MIN_ANGLE,
                 ProtocolCode.GET_JOINT_MAX_ANGLE,