Preliminary adaptation mypalletizer

Author: anla-xu

.gitignore

@@ -11,7 +11,7 @@ record.txt
 test_test.py
 
 .DS_Store
-
+.history
 .VSCodeCounter
 .vscode/.ropeproject/config.py
 .vscode/.ropeproject/objectdb

pymycobot/common.py

@@ -109,7 +109,8 @@ def _int2coord(self, _int):
 
     def _flatten(self, _list):
         return sum(
-            ([x] if not isinstance(x, list) else self._flatten(x) for x in _list), []
+            ([x] if not isinstance(x, list) else self._flatten(x)
+             for x in _list), []
         )
 
     def _process_data_command(self, args):
@@ -118,7 +119,8 @@ def _process_data_command(self, args):
 
         return self._flatten(
             [
-                [self._encode_int16(int(i)) for i in x] if isinstance(x, list) else x
+                [self._encode_int16(int(i))
+                 for i in x] if isinstance(x, list) else x
                 for x in args
             ]
         )
@@ -135,24 +137,24 @@ def _process_received(self, data, genre):
         # Get valid header: 0xfe0xfe
         for idx in range(data_len - 1):
             if self._is_frame_header(data, idx):
-                break
+                data_len = data[idx + 2] - 2
+                if data_len > 0:
+                    break
         else:
             return []
 
-        data_len = data[idx + 2] - 2
-
         # compare send header and received header
         cmd_id = data[idx + 3]
         if cmd_id != genre:
             return []
         data_pos = idx + 4
-        valid_data = data[data_pos : data_pos + data_len]
+        valid_data = data[data_pos: data_pos + data_len]
 
         # process valid data
         res = []
         if data_len == 12 or data_len == 8:
             for idx in range(0, len(valid_data), 2):
-                one = valid_data[idx : idx + 2]
+                one = valid_data[idx: idx + 2]
                 res.append(self._decode_int16(one))
         elif data_len == 2:
             if genre in [ProtocolCode.IS_SERVO_ENABLE]:

pymycobot/generate.py

@@ -4,6 +4,7 @@
 import logging
 
 from pymycobot.log import setup_logging
+from pymycobot.error import calibration_parameters
 from pymycobot.common import ProtocolCode, DataProcessor
 
 

pymycobot/mycobot.py

@@ -79,7 +79,8 @@ def _mesg(self, genre, *args, **kwargs):
             **kwargs: support `has_reply`
                 has_reply: Whether there is a return value to accept.
         """
-        real_command, has_reply = super(MyCobot, self)._mesg(genre, *args, **kwargs)
+        real_command, has_reply = super(
+            MyCobot, self)._mesg(genre, *args, **kwargs)
         self._write(self._flatten(real_command))
 
         if has_reply:
@@ -140,7 +141,8 @@ def send_radians(self, radians, speed):
             speed (int): 0 ~ 100
         """
         calibration_parameters(len6=radians, speed=speed)
-        degrees = [self._angle2int(radian * (180 / math.pi)) for radian in radians]
+        degrees = [self._angle2int(radian * (180 / math.pi))
+                   for radian in radians]
         return self._mesg(ProtocolCode.SEND_ANGLES, degrees, speed)
 
     def sync_send_angles(self, degrees, speed, timeout=7):

pymycobot/mypalletizer.py

@@ -7,10 +7,77 @@
 from .common import ProtocolCode, read, write
 
 
-def calibration_parameters(**kwargs):
+class MyPalletizedataException(Exception):
     pass
 
 
+MIN_ID = 0
+MAX_ID = 5
+
+# In fact, most joints cannot reach plus or minus 180 degrees.
+# There may be a value greater than 180 when reading the angle,
+# and the maximum and minimum values are expanded for compatibility.
+MIN_ANGLE = -170.0
+MAX_ANGLE = 170.0
+
+
+def calibration_parameters(**kwargs):
+    if kwargs.get("id", None) is not None and not MIN_ID <= kwargs["id"] <= MAX_ID:
+        raise MyPalletizedataException(
+            "The id not right, should be {0} ~ {1}, but received {2}.".format(
+                MIN_ID, MAX_ID, kwargs["id"]
+            )
+        )
+
+    if (
+        kwargs.get("degree", None) is not None
+        and not MIN_ANGLE <= kwargs["degree"] <= MAX_ANGLE
+    ):
+        raise MyPalletizedataException(
+            "degree value not right, should be {0} ~ {1}, but received {2}".format(
+                MIN_ANGLE, MAX_ANGLE, kwargs["degree"]
+            )
+        )
+
+    if kwargs.get("degrees", None) is not None:
+        degrees = kwargs["degrees"]
+        if not isinstance(degrees, list):
+            raise MyPalletizedataException("`degrees` must be a list.")
+        if len(degrees) != 4:
+            raise MyPalletizedataException(
+                "The length of `degrees` must be 4.")
+        for idx, angle in enumerate(degrees):
+            if not MIN_ANGLE <= angle <= MAX_ANGLE:
+                raise MyPalletizedataException(
+                    "Has invalid degree value, error on index {0}. Degree should be {1} ~ {2}.".format(
+                        idx, MIN_ANGLE, MAX_ANGLE
+                    )
+                )
+
+    if kwargs.get("coords", None) is not None:
+        coords = kwargs["coords"]
+        if not isinstance(coords, list):
+            raise MyPalletizedataException("`coords` must be a list.")
+        if len(coords) != 4:
+            raise MyPalletizedataException(
+                "The length of `coords` must be 4.")
+
+    if kwargs.get("speed", None) is not None and not 0 <= kwargs["speed"] <= 100:
+        raise MyPalletizedataException(
+            "speed value not right, should be 0 ~ 100, the error speed is %s"
+            % kwargs["speed"]
+        )
+
+    if kwargs.get("rgb", None) is not None:
+        rgb_str = ["r", "g", "b"]
+        for i, v in enumerate(kwargs["rgb"]):
+            if not (0 <= v <= 255):
+                raise MyPalletizedataException(
+                    "The RGB value needs be 0 ~ 255, but the %s is %s" % (
+                        rgb_str[i], v)
+                )
+
+
 class MyPalletizer(MycobotCommandGenerater):
     def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
         """
@@ -24,7 +91,6 @@ def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
         self.debug = debug
         setup_logging(self.debug)
         self.log = logging.getLogger(__name__)
-
         self.calibration_parameters = calibration_parameters
 
         import serial