fix(Mercury): Resolved server communication issues affecting both arms (#210)

* add MyCobot 280 X5PI API

* add MyCobot 280 X5 PI doc

* Fix MyCobot 280 X5 Pi doc errors

* add MyCobot 280 X5 PI sokcet sever&client

* MyCobot 280 X5 PI server compatible with python2

* rename MyCobot280x5pi to MyCobot280RDKX5

* fix MyArmMControl bugs

* fix MyArmC bugs

* fix myArmM&C demo bug

* update M&C demo

* fix MyCobot280 rdkx5 bug

* fix MyArmM&C bugs

* fix bugs

* fix bug

* fix ThreeHand api bug

* Fixed the issue that Pro630 could not read data

* fix MyArmM bug

* MyArmM&C adds get_joints_coord interface

* fix Pro630 bug

* fix Pro630 bugs

* Fixed the pro630 socket communication bug

* Optimize MyCobot280 RDK X5 interface parameter error prompt

* Optimized the way MyAGV reads MCU data

* feat(myagvpro): add myagvpro api

* fix(ConveyorApi):Fix the problem that no data is returned

* feat(MyAGVPro): Adapt to Bluetooth and socket TCP communication modes

* docs(MyAGVPro): Format the document

* docs(MyAGVPro): Documentation to increase use cases

* docs(MyAGVPro): rename MyAGVPro_zh.md

* docs(MyAGVPro): Update the serial port

* fix(MyArmM): Correction of joint limit and motor code value range

* feat(MyAGVPro): Add get_emergency_stop_state api

* feat(MyAGVPro): Add get_emergency_stop_state api

* refactor(MyAGVPro): rename get_emergency_stop_state to get_estop_state

* feat(MyAgv): Added setting/reading automatic report status interface

* fix(M750):Updated M750 Swing Limit

* fix(MyAGVPro): The issue that Bluetooth address matching fails is fixed

* feat(myagvpro): add get_robot_status api

* feat(demo):Add myAGVPro composite kit handle control case

* feat(MercuryX1): Compatible with dock firmware version 1.2

* feat(mercurychassis_api): Dynamically matching instruction length

* feat(mercurychassis_api): Optimize voltage reading accuracy

* feat(ConveyorApi): Separate the set_motor_speed interface

* feat(ConveyorApi): Specify the motor type during initialization

* refactor(conveyorApi): add stop interface

* Develop branch merge (#196)

* 添加pid设置demo

* chore: Add a custom pylint configuration file

---------

Co-authored-by: weiquan

* feat(conveyor_api): Adapt to Conveyor 1.1 firmware, all interfaces return uniform values

* feat(AGVPRO): Adapt to CH340 serial port chip

* feat(AGVPRO): Automatically upload the adapter firmware v1.0.10

* feat(AGVPRO): Added the control interface for opening and closing the handle

* feat(AGVPRO): Added an interface for reading handle control status

* feat(AGVPRO): Bluetooth adapter handle control interface

* fix(mercury): Fix the redundant code in the previous commit

* fix(Mercury): Resolved server communication issues affecting both arms

---------

Co-authored-by: Mrkun5018 <[email protected]>
Co-authored-by: weiquan <[email protected]>

Author: K-Shuffler

pymycobot/close_loop.py

@@ -105,11 +105,16 @@ def _mesg(self, genre, *args, **kwargs):
 
         need_break = False
         data = None
-        timeout = 0.5
+
         if self.__class__.__name__ == "MercurySocket":
             timeout = 1
         elif self.__class__.__name__ == "Pro450Client":
             timeout = 3
+        elif self.__class__.__name__ == "MercuryArmsSocket":
+            timeout = 1
+        else:
+            timeout = 0.5
+
         interval_time = time.time()
         is_moving = 0
         check_is_moving_t = 1

pymycobot/mercury_arms_socket.py

@@ -3,14 +3,32 @@
 import json
 import struct
 import socket
+import sys
 import threading
+import time
+import traceback
 from pymycobot.mercury_api import MercuryCommandGenerator
 from pymycobot.error import calibration_parameters
 
 """
 for mercury x1 robot arms (six-axis or seven-axis), using socket to communicate with the robot
 """
 
+SYS_VERSION_INFO = sys.version_info.major
+
+
+def format_hex_log(data):
+    if SYS_VERSION_INFO == 2:
+        command_log = ""
+        for d in data:
+            command_log += hex(ord(d))[2:] + " "
+    else:
+        command_log = ""
+        for d in data:
+            command_log += hex(d)[2:] + " "
+
+    return command_log
+
 
 class MercuryArmsSocket(MercuryCommandGenerator):
     def __init__(self, arm, ip, netport=9000, debug=False):
@@ -23,17 +41,39 @@ def __init__(self, arm, ip, netport=9000, debug=False):
             debug: debug mode
         """
         super(MercuryArmsSocket, self).__init__(debug)
+        self.crc_robot_class.append(self.__class__.__name__)
         self.arm = arm
         self.calibration_parameters = calibration_parameters
         self.SERVER_IP = ip
         self.SERVER_PORT = netport
         self.sock = self.connect_socket()
 
-        self.read_threading = threading.Thread(target=self.read_thread, args=("socket", ))
+        self.read_threading = threading.Thread(target=self.read_thread, args=("socket",))
         self.read_threading.daemon = True
         self.read_threading.start()
         self.get_limit_switch()
 
+    def read_thread(self, method=None):
+        self.sock.settimeout(3)
+        while True:
+            try:
+                data = self.sock.recv(1024)
+                result = self._process_received(data)
+
+                self.log.debug(f"_read :{format_hex_log(result)}")
+                if not result:
+                    continue
+
+                with self.lock:
+                    self.read_command.append([result, time.time()])
+
+            except socket.timeout:
+                time.sleep(0.1)
+
+            except Exception as e:
+                print(e)
+                traceback.print_exc()
+
     def connect_socket(self):
         sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
         sock.connect((self.SERVER_IP, self.SERVER_PORT))
@@ -53,33 +93,17 @@ def __format(self, command: str) -> bytes:
         return b''.join([date_length, data_byter])
 
     def _write(self, command, method=None):
-        log_command = " ".join(map(lambda n: hex(n)[2:], command))
-        self.log.debug("_write: {}".format(log_command))
+        self.log.debug("_write: {}".format(format_hex_log(command)))
         self.sock.sendall(self.__format(command))
 
+    def _send_command(self, genre, real_command):
+        self.write_command.append(genre)
+        self._write(self._flatten(real_command), method="socket")
 
 
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+if __name__ == '__main__':
+    mercury_socket = MercuryArmsSocket("left_arm", ip="192.168.1.216", debug=True)
+    mercury_socket.send_angle(1, 20, 10)
+    # while True:
+    #     print(mercury_socket.get_angles())
+    #     time.sleep(0.1)