add mybuddy socket and bluetooth

Author: anla-xu

pymycobot/bluet.py

@@ -0,0 +1,44 @@
+# coding: utf-8
+import bluetooth
+import sys
+
+class BluetoothConnection:
+    def __init__(self):
+        self.device = []
+        self.target_name = "mybuddy"
+        self.nearby_devices = None
+ 
+    def find_target_device(self):
+        available_addr = []
+        self.nearby_devices = bluetooth.discover_devices(lookup_names=True, duration=5)
+        if self.nearby_devices:
+            for addr, name in self.nearby_devices:
+                if self.target_name == name:
+                    available_addr.append(addr)
+            return available_addr
+        return None
+ 
+    def connect_target_device(self):
+        target_address = self.find_target_device()
+        if target_address:
+            if len(target_address) > 1:
+                device_info = ""
+                i = 1
+                sys.stdout.write("please select the device you want to connect:\n".format(4))
+                for addr, name in target_address:
+                    device_info += "{} >>> {} - {} \n".format(i,addr, name)
+                sys.stdout.write(device_info)
+                choose_device = input("please enter 1-{}:".format(len(target_address)))
+                target_address = target_address[int(choose_device)-1][0]
+            sock = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
+            try:
+                sock.connect((target_address[0][0], 1))
+                return sock
+            except Exception as e:
+                # print("connection fail\n", e)
+                sock.close()
+                return None
+        return None
+                
+if __name__ == "__main__":
+    pass

pymycobot/generate.py

@@ -214,6 +214,7 @@ def send_angle(self, id, degree, speed):
             id : Joint id(genre.Angle)\n
                     For mycobot: int 1-6.\n
                     For mypalletizer: int 1-4.
+                    For mypalletizer 340: int 1-3.
             degree : degree value(float)(about -170 ~ 170).
             speed : (int) 0 ~ 100
         """
@@ -228,6 +229,7 @@ def send_angles(self, degrees, speed):
             degrees: a list of degree values(List[float]), length 6 or 4.\n
                         for mycobot: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0].\n
                         for mypalletizer: [0.0, 0.0, 0.0, 0.0]
+                        for mypalletizer 340: [0.0, 0.0, 0.0]
             speed : (int) 0 ~ 100
         """
         self.calibration_parameters(degrees=degrees, speed=speed)
@@ -241,6 +243,7 @@ def get_coords(self):
             list : A float list of coord 
                 for mycobot: [x, y, z, rx, ry, rz]\n
                 for mypalletizer: [x, y, z, θ]
+                for mypalletizer 340: [x, y, z]
         """
         return self._mesg(ProtocolCode.GET_COORDS, has_reply=True)
 
@@ -251,24 +254,26 @@ def send_coord(self, id, coord, speed):
             id(int) : coord id(genre.Coord)\n
                         For mycobot: int 1-6.\n
                         For mypalletizer: int 1-4.
+                        For mypalletizer 340: int 1-3.
             coord(float) : coord value, mm
             speed(int) : 0 ~ 100
         """
-        self.calibration_parameters(id=id, speed=speed)
+        # self.calibration_parameters(id=id, speed=speed)
         value = self._coord2int(coord) if id <= 3 else self._angle2int(coord)
         return self._mesg(ProtocolCode.SEND_COORD, id, [value], speed)
 
     def send_coords(self, coords, speed, mode):
         """Send all coords to robot arm.
 
         Args:
-            coords: a list of coords value(List[float]), length 6 or 4.
+            coords: a list of coords value(List[float]).
                         for mycobot :[x(mm), y, z, rx(angle), ry, rz]\n
                         for mypalletizer: [x, y, z, θ]
+                        for mypalletizer 340: [x, y, z]
             speed : (int) 0 ~ 100
             mode : (int) 0 - angluar, 1 - linear
         """
-        self.calibration_parameters(coords=coords, speed=speed)
+        # self.calibration_parameters(coords=coords, speed=speed)
         coord_list = []
         for idx in range(3):
             coord_list.append(self._coord2int(coords[idx]))
@@ -283,6 +288,7 @@ def is_in_position(self, data, id=0):
             data: A data list, angles or coords.
                     for mycobot: len 6.
                     for mypalletizer: len 4
+                    for mypalletizer 340: len 3
             id: 1 - coords, 0 - angles
 
         Return:
@@ -291,14 +297,14 @@ def is_in_position(self, data, id=0):
             -1 - Error
         """
         if id == 1:
-            self.calibration_parameters(coords=data)
+            # self.calibration_parameters(coords=data)
             data_list = []
             for idx in range(3):
                 data_list.append(self._coord2int(data[idx]))
             for idx in range(3, 6):
                 data_list.append(self._angle2int(data[idx]))
         elif id == 0:
-            self.calibration_parameters(degrees=data)
+            # self.calibration_parameters(degrees=data)
             data_list = [self._angle2int(i) for i in data]
         else:
             raise Exception("id is not right, please input 0 or 1")
@@ -323,6 +329,7 @@ def jog_angle(self, joint_id, direction, speed):
             joint_id:
                     For mycobot: int 1-6.\n
                     For mypalletizer: int 1-4.
+                    For mypalletizer 340: int 1-3.
             direction: 0 - decrease, 1 - increase
             speed: int (0 - 100)
         """
@@ -335,6 +342,7 @@ def jog_coord(self, coord_id, direction, speed):
             coord_id:
                     For mycobot: int 1-6.\n
                     For mypalletizer: int 1-4.
+                    For mypalletizer 340: int 1-3.
             direction: 0 - decrease, 1 - increase
             speed: int (0 - 100)
         """
@@ -395,6 +403,7 @@ def set_encoder(self, joint_id, encoder):
             joint_id: 
                 for mycobot: Joint id 1 - 6
                 for mypalletizer: Joint id 1 - 4
+                for mypalletizer 340: Joint id 1 - 3
                 for mycobot gripper: Joint id 7
             encoder: The value of the set encoder.
         """
@@ -564,6 +573,7 @@ def release_servo(self, servo_id):
             servo_id:
                 for mycobot: 1 - 6.\n
                 for mypalletizer: 1 - 4
+                for mypalletizer 340: 1 - 3
         """
         return self._mesg(ProtocolCode.RELEASE_SERVO, servo_id)
 
@@ -574,6 +584,7 @@ def focus_servo(self, servo_id):
             servo_id:
                 for mycobot: 1 - 6\n
                 for mypalletizer: 1 - 4
+                for mypalletizer 340: 1 - 3
         """
         return self._mesg(ProtocolCode.FOCUS_SERVO, servo_id)
 

pymycobot/mybuddybluetooth.py

@@ -0,0 +1,86 @@
+# coding: utf-8
+import bluetooth
+
+from pymycobot.Interface import MyBuddyCommandGenerator
+from pymycobot.common import ProtocolCode, write, read
+from pymycobot.error import calibration_parameters
+from pymycobot.bluet import BluetoothConnection
+
+
+class MyBuddyBlueTooth(MyBuddyCommandGenerator):
+    """MyBuddy bluetooth API"""
+    def __init__(self):
+        """There is a default Bluetooth search time of 5 seconds"""
+        super(MyBuddyBlueTooth).__init__()
+        self.ble = BluetoothConnection()
+        self.ble_obj = self.ble.connect_target_device()
+        
+    def __read(self):
+        while True:
+            data = self.ble_obj.recv(1024)
+            if data:
+                return data
+        
+        
+    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.
+        """
+        real_command, has_reply = super(
+            MyBuddyBlueTooth, self)._mesg(genre, *args, **kwargs)
+        self.ble_obj.write(bytes(self._flatten(real_command)))
+
+        if has_reply:
+            data = self.__read()
+            res = self._process_received(data, genre)
+            if genre in [
+                ProtocolCode.ROBOT_VERSION,
+                ProtocolCode.IS_POWER_ON,
+                ProtocolCode.IS_CONTROLLER_CONNECTED,
+                ProtocolCode.IS_PAUSED,  # TODO have bug: return b''
+                ProtocolCode.IS_IN_POSITION,
+                ProtocolCode.IS_MOVING,
+                ProtocolCode.IS_SERVO_ENABLE,
+                ProtocolCode.IS_ALL_SERVO_ENABLE,
+                ProtocolCode.GET_SERVO_DATA,
+                ProtocolCode.GET_DIGITAL_INPUT,
+                ProtocolCode.GET_GRIPPER_VALUE,
+                ProtocolCode.IS_GRIPPER_MOVING,
+                ProtocolCode.GET_SPEED,
+                ProtocolCode.GET_ENCODER,
+                ProtocolCode.GET_BASIC_INPUT,
+                ProtocolCode.GET_TOF_DISTANCE,
+                ProtocolCode.GET_END_TYPE,
+                ProtocolCode.GET_MOVEMENT_TYPE,
+                ProtocolCode.GET_REFERENCE_FRAME,
+                ProtocolCode.GET_JOINT_MIN_ANGLE,
+                ProtocolCode.GET_JOINT_MAX_ANGLE
+            ]:
+                return self._process_single(res)
+            elif genre in [ProtocolCode.GET_ANGLES]:
+                return [self._int2angle(angle) for angle in res]
+            elif genre in [ProtocolCode.GET_COORDS, ProtocolCode.GET_TOOL_REFERENCE, ProtocolCode.GET_WORLD_REFERENCE]:
+                if res:
+                    r = []
+                    for idx in range(3):
+                        r.append(self._int2coord(res[idx]))
+                    for idx in range(3, 6):
+                        r.append(self._int2angle(res[idx]))
+                    return r
+                else:
+                    return res
+            elif genre in [ProtocolCode.GET_SERVO_VOLTAGES]:
+                return [self._int2coord(angle) for angle in res]
+            else:
+                return res
+        return None
+        
+