refactor: imporve api

Author: zlj-zz

pymycobot/common.py

@@ -9,6 +9,7 @@ class Command():
     FOOTER = 0xfa
 
     # Overall status
+    VERSION = 0x00
     POWER_ON = 0x10
     POWER_OFF = 0x11
     IS_POWER_ON = 0x12
@@ -89,6 +90,56 @@ def _flatten(self, _list):
         return sum(([x] if not isinstance(x, list) else self._flatten(x)
                     for x in _list), [])
 
+    def _process_data_command(self, args):
+        if not args:
+            return []
+
+        return self._flatten([
+                [self._encode_int16(i) for i in x]
+                if isinstance(x, list) else x
+                    for x in args])
+
+    def _is_frame_header(self, data, pos):
+        return data[pos] == Command.HEADER and data[pos + 1] == Command.HEADER
+
+    def _process_recived(self, data, genre):
+        if not data:
+            return []
+
+        data = bytearray(data)
+        data_len = len(data)
+        # Get valid header: 0xfe0xfe
+        for idx in range(data_len - 1):
+            if self._is_frame_header(data, idx):
+                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])
+
+        # process valid data
+        res = []
+        if data_len == 12:
+            for idx in range(0, len(valid_data), 2):
+                one = valid_data[idx:idx + 2]
+                res.append(self._decode_int16(one))
+        elif data_len == 2:
+            res.append(self._decode_int16(valid_data))
+        else:
+            res.append(self._decode_int8(valid_data))
+        return res
+
+    def _process_single(self, data):
+        return data[0] if data else -1
+
+'''
     def _process_data_command(self, data, genre):
         # lenght = 0
         if not data:
@@ -133,41 +184,4 @@ def _process_data_command(self, data, genre):
         # lenght = 1
         else:
             return [data[0]]
-
-    def _is_frame_header(self, data, pos):
-        return data[pos] == Command.HEADER and data[pos + 1] == Command.HEADER
-
-    def _process_recived(self, data, genre):
-        if not data:
-            return []
-
-        data = bytearray(data)
-        data_len = len(data)
-        for idx in range(data_len - 1):
-            if self._is_frame_header(data, idx):
-                break
-        else:
-            return []
-
-        data_len = data[idx + 2] - 2
-        cmd_id = data[idx + 3]
-        if cmd_id != genre:
-            return []
-        data_pos = idx + 4
-        valid_data = (data[data_pos:data_pos + data_len])
-        # print(data_len)
-        # print(valid_data)
-
-        res = []
-        if data_len == 12:
-            for idx in range(0, len(valid_data), 2):
-                one = valid_data[idx:idx + 2]
-                res.append(self._decode_int16(one))
-        elif data_len == 2:
-            res.append(self._decode_int16(valid_data))
-        else:
-            res.append(self._decode_int8(valid_data))
-        return res
-
-    def _process_single(self, data):
-        return data[0] if data else -1
+'''

pymycobot/mycobot.py

@@ -12,6 +12,7 @@ class MyCobot(MyCobotData):
     Possessed function:
 
         # Overall status
+            version()
             power_on()
             power_off()
             is_power_on()
@@ -64,8 +65,17 @@ class MyCobot(MyCobotData):
             is_gripper_moving()
     '''
 
-    def __init__(self, port, boudrate='115200', timeout=0.1):
+    def __init__(self, port, boudrate='115200', timeout=0.1, debug=False):
+        '''
+        Args:
+            port     : port string
+            boudrate : baud rate string, default '115200'
+            timeout  : default 0.1
+            debug    : whether show debug info
+
+        '''
         self._version = sys.version_info[:2][0]
+        self.debug = debug
         for _ in range(5):
             try:
                 self._serial_port = serial.Serial(port,
@@ -80,7 +90,12 @@ def __init__(self, port, boudrate='115200', timeout=0.1):
             print('Connect prot failed, eixt.')
             exit(0)
 
+    def _debug_log(self,  info, _type=None):
+        print('[debug] {}: {}'.format(_type, info))
+
     def _write(self, command):
+        if self.debug:
+            self._debug_log(command, 'serial will write')
         self._serial_port.write(command)
         self._serial_port.flush()
         time.sleep(0.05)
@@ -89,24 +104,52 @@ def _read(self, size=1024):
         if self._serial_port.inWaiting() > 0:
             data = self._serial_port.read(self._serial_port.inWaiting())
         else:
+            if self.debug:
+                self._debug_log('no data can be read', 'warn')
             data = None
         return data
 
-    def __mesg(self, genre, data=None, has_reply=False):
-        command_data = self._process_data_command(data, genre)
+    def __mesg(self, genre, *args, **kwargs):
+        '''
+
+        Args:
+            genre: command type (Command)
+            *args: other data.
+                   It is converted to octal by default. 
+                   If the data needs to be encapsulated into hexadecimal, 
+                   the array is used to include them. (Data cannot be nested)
+            **kwargs: support `has_reply`
+                has_reply: Whether there is a return value to accept.
+
+        '''
+        command_data = self._process_data_command(args)
+        if self.debug:
+            self._debug_log(command_data, 'processed data')
+
         LEN = len(command_data) + 2
         command = [
             Command.HEADER, Command.HEADER, LEN, genre, command_data,
             Command.FOOTER
         ]
         self._write(self._flatten(command))
 
-        if has_reply:
+        if 'has_reply' in kwargs and kwargs['has_reply']:
             data = self._read()
             res = self._process_recived(data, genre)
             return res
 
     # Overall status
+    def version(self): # TODO: test method <11-03-21, yourname> #
+        '''Get cobot verion
+
+        Return:
+            mycobot   : 1
+            mycobotPro: 101
+
+        '''
+        recv = self.__mesg(Command.VERSION, has_reply=True)
+        return recv
+
     def power_on(self):
         self.__mesg(Command.POWER_ON)
 
@@ -150,8 +193,8 @@ def send_angle(self, id, degree, speed):
             speed (int): 0 ~100
 
         '''
-        data = [id - 1, self._angle_to_int(degree), speed]
-        self.__mesg(Command.SEND_ANGLE, data=data)
+        self.__mesg(Command.SEND_ANGLE, id - 1, [self._angle_to_int(degree)],
+                    speed)
 
     def send_angles(self, degrees, speed):
         '''Send all angles
@@ -162,8 +205,8 @@ def send_angles(self, degrees, speed):
 
         '''
         degrees = [self._angle_to_int(degree) for degree in degrees]
-        data = [degrees, speed]
-        self.__mesg(Command.SEND_ANGLES, data=self._flatten(data))
+        # data = [degrees, speed]
+        self.__mesg(Command.SEND_ANGLES, degrees, speed)
 
     def get_radians(self):
         '''Get all angle return a list
@@ -189,8 +232,7 @@ def send_radians(self, radians, speed):
         degrees = [
             self._angle_to_int(radian * (180 / math.pi)) for radian in radians
         ]
-        return self.__mesg(Command.SEND_ANGLES,
-                          data=self._flatten([degrees, speed]))
+        return self.__mesg(Command.SEND_ANGLES, degrees, speed)
 
     def get_coords(self):
         '''Get all coords.
@@ -220,9 +262,8 @@ def send_coord(self, id, coord, speed):
             speed(int):
 
         '''
-        return self.__mesg(Command.SEND_COORD,
-                          data=[id - 1,
-                                self._coord_to_int(coord), speed])
+        return self.__mesg(Command.SEND_COORD, id - 1,
+                           [self._coord_to_int(coord)], speed)
 
     def send_coords(self, coords, speed, mode):
         '''Send all coords
@@ -241,8 +282,7 @@ def send_coords(self, coords, speed, mode):
             coord_list.append(self._coord_to_int(coords[idx]))
         for idx in range(3, 6):
             coord_list.append(self._angle_to_int(coords[idx]))
-        self.__mesg(Command.SEND_COORDS,
-                   data=self._flatten([coord_list, speed, mode]))
+        self.__mesg(Command.SEND_COORDS, coord_list, speed, mode)
 
     def pause(self):
         self.__mesg(Command.PAUSE)
@@ -283,8 +323,9 @@ def is_in_position(self, data, id):
             raise MyCobotDataException("id is not right, please input 0 or 1")
 
         received = self.__mesg(Command.IS_IN_POSITION,
-                              data=data_list,
-                              has_reply=True)
+                               data_list,
+                               id,
+                               has_reply=True)
         return self._process_single(received)
 
     def is_moving(self):
@@ -308,7 +349,7 @@ def jog_angle(self, joint_id, direction, speed):
             direction: int [0, 1]
             speed: int (0 - 100)
         '''
-        self.__mesg(Command.JOG_ANGLE, data=[joint_id, direction, speed])
+        self.__mesg(Command.JOG_ANGLE, joint_id, direction, speed)
 
     @check_id
     def jog_coord(self, coord_id, direction, speed):
@@ -319,7 +360,7 @@ def jog_coord(self, coord_id, direction, speed):
             direction: int [0, 1]
             speed: int (0 - 100)
         '''
-        self.__mesg(Command.JOG_COORD, data=[coord_id, direction, speed])
+        self.__mesg(Command.JOG_COORD, coord_id, direction, speed)
 
     def jog_stop(self):
         self.__mesg(Command.JOG_STOP)
@@ -339,27 +380,27 @@ def set_speed(self, speed):
             raise MyCobotDataException(
                 'speed value not right, should be 0 ~ 100 ')
 
-        self.__mesg(Command.SET_SPEED, data=[speed])
+        self.__mesg(Command.SET_SPEED, speed)
 
     @check_id
     def get_joint_min_angle(self, joint_id):
         angle = self.__mesg(Command.GET_JOINT_MIN_ANGLE,
-                           data=[joint_id],
+                           joint_id,
                            has_reply=True)
         return self._int_to_angle(angle[0]) if angle else 0
 
     @check_id
     def get_joint_max_angle(self, joint_id):
         angle = self.__mesg(Command.GET_JOINT_MAX_ANGLE,
-                           data=[joint_id],
+                           joint_id,
                            has_reply=True)
         return self._int_to_angle(angle[0]) if angle else 0
 
     # Servo control
     @check_id
     def is_servo_enable(self, servo_id):
         return self._process_single(
-            self.__mesg(Command.IS_SERVO_ENABLE, data=[servo_id]))
+            self.__mesg(Command.IS_SERVO_ENABLE, servo_id))
 
     def is_all_servo_enable(self):
         return self._process_single(
@@ -384,6 +425,18 @@ def focus_servo(self, servo_id):
         self.__mesg(Command.RELEASE_SERVO, servo_id)
 
     # Atom IO
+    def set_color(self, r, g, b):
+        '''Set the light color
+
+        Args:
+            rgs (str): example 'ff0000'
+            r (int): 0 ~ 255
+            g (int): 0 ~ 255
+            b (int): 0 ~ 255
+
+        '''
+        self.__mesg(Command.SET_COLOR, r, g, b)
+
     def set_led_color(self, rgb):
         '''Set the light color
 
@@ -413,7 +466,7 @@ def set_pin_mode(self, pin_no, pin_mode):
             pin_mode (int): 0 - input, 1 - output, 2 - input_pullup
 
         '''
-        self.__mesg(Command.SET_PIN_MODE, data=[pin_no, pin_mode])
+        self.__mesg(Command.SET_PIN_MODE, pin_no, pin_mode)
 
     def set_digital_output(self, pin_no, pin_signal):
         '''
@@ -423,19 +476,19 @@ def set_digital_output(self, pin_no, pin_signal):
             pin_signal (int): 0 / 1
 
         '''
-        self.__mesg(Command.SET_DIGITAL_OUTPUT, data=[pin_no, pin_signal])
+        self.__mesg(Command.SET_DIGITAL_OUTPUT, pin_no, pin_signal)
 
     def get_digital_input(self, pin_no):
         return self._process_single(
             self.__mesg(Command.GET_DIGITAL_INPUT,
-                        data=[pin_no],
+                        pin_no,
                         has_reply=True))
 
     def set_pwm_mode(self, pin_no, channel):
-        self.__mesg(Command.SET_PWM_MODE, data=[pin_no, channel])
+        self.__mesg(Command.SET_PWM_MODE, pin_no, channel)
 
     def set_pwm_output(self, channel, pin_val):
-        self.__mesg(Command.SET_PWM_OUTPUT, data=[channel, pin_val])
+        self.__mesg(Command.SET_PWM_OUTPUT, channel, pin_val)
 
     def get_gripper_value(self):
         return self._process_single(
@@ -452,7 +505,7 @@ def set_gripper_state(self, flag, speed):
         if not flag in [0, 1]:
             raise MyCobotDataException('eror flag, please input 0 or 1')
 
-        self.__mesg(Command.SET_GRIPPER_STATE, data=[flag, speed])
+        self.__mesg(Command.SET_GRIPPER_STATE, flag, speed)
 
     def set_gripper_value(self, value, speed):
         '''Set gripper value
@@ -462,7 +515,7 @@ def set_gripper_value(self, value, speed):
             speed (int): 0 ~ 100
 
         '''
-        self.__mesg(Command.SET_GRIPPER_VALUE, data=[value, speed])
+        self.__mesg(Command.SET_GRIPPER_VALUE, value, speed)
 
     def set_gripper_ini(self):
         '''Set the current position to zero