Added network communication and distance detection functions for m5 and seed versions

Author: anla-xu

.gitignore

@@ -15,6 +15,7 @@ test_test.py
 *.bat
 .VSCodeCounter
 .vscode/
+*.sh
 
 *.pyc
 __pycache__/port.cpython-39.pyc

demo/Client.py

@@ -1,6 +1,3 @@
-#!/usr/bin/env python3
-# coding:utf-8
-
 from pymycobot import MyCobotSocket
 
 m = MyCobotSocket("192.168.10.10", "9000")

demo/Server.py

@@ -20,28 +20,30 @@ def get_logger(name):
     # console = logging.StreamHandler()
     # console.setFormatter(formatter)
 
-    save = logging.handlers.RotatingFileHandler("/home/ubuntu/mycobot_server.log", maxBytes=10485760, backupCount=1)
+    save = logging.handlers.RotatingFileHandler(
+        "/home/ubuntu/mycobot_server.log", maxBytes=10485760, backupCount=1)
     save.setFormatter(formatter)
 
     logger.addHandler(save)
     # logger.addHandler(console)
     return logger
 
+
 class MycobotServer(object):
 
     def __init__(self, host, port):
         GPIO.setwarnings(False)
         self.logger = get_logger("AS")
         self.mc = None
         self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-        self.s.bind((host,port))
+        self.s.bind((host, port))
         print "Binding succeeded!"
         self.s.listen(1)
         self.connect()
 
     def connect(self):
-        try:
-            while True:
+        while True:
+            try:
                 print "waiting connect!------------------"
                 conn, addr = self.s.accept()
                 port_baud = []
@@ -54,12 +56,13 @@ def connect(self):
                             print("close dsiconnect!")
                             break
                         res = b'1'
-                        command = command.replace(" ","")
-                        if len(port_baud)<3:
-                            
+                        command = command.replace(" ", "")
+                        if len(port_baud) < 3:
+
                             port_baud.append(command)
-                            if len(port_baud)==3:
-                                self.mc = serial.Serial(port_baud[0],port_baud[1],timeout=float(port_baud[1]))
+                            if len(port_baud) == 3:
+                                self.mc = serial.Serial(
+                                    port_baud[0], port_baud[1], timeout=float(port_baud[1]))
                                 port_baud.append(1)
                         else:
                             self.logger.info(command)
@@ -71,12 +74,12 @@ def connect(self):
                                     GPIO.setmode(GPIO.BOARD)
                             elif command[3] == 171:
                                 if command[5]:
-                                    GPIO.setup(command[4],GPIO.OUT)
+                                    GPIO.setup(command[4], GPIO.OUT)
                                 else:
-                                    GPIO.setup(command[4],GPIO.IN)
+                                    GPIO.setup(command[4], GPIO.IN)
 
                             elif command[3] == 172:
-                                GPIO.output(command[4],command[5])
+                                GPIO.output(command[4], command[5])
 
                             elif command[3] == 173:
                                 res = bytes(GPIO.input(command[4]))
@@ -89,19 +92,19 @@ def connect(self):
                     except Exception as e:
                         self.logger.error(str(e))
                         conn.sendall(str.encode("ERROR:"+str(e)))
-                        break          
-        except Exception as e:
-            self.logger.error(str(e))
-            conn.close()
+                        break
+            except Exception as e:
+                self.logger.error(str(e))
+                conn.close()
 
-    def write(self,command):
+    def write(self, command):
         self.mc.write(command)
         self.mc.flush()
         time.sleep(0.05)
 
     def read(self):
         data = None
-        if self.mc.inWaiting()>0:
+        if self.mc.inWaiting() > 0:
             data = self.mc.read(self.mc.inWaiting())
         return data
 
@@ -112,7 +115,8 @@ def re_data_2(self, command):
         data_list = [int(i) for i in data_list]
         return data_list
 
+
 if __name__ == "__main__":
     HOST = socket.gethostbyname(socket.gethostname())
     PORT = 9000
-    MycobotServer(HOST,PORT)
+    MycobotServer(HOST, PORT)

pymycobot/common.py

@@ -86,6 +86,14 @@ class ProtocolCode(object):
     SET_GPIO_OUTPUT = 0xAC
     GET_GPIO_IN = 0xAD
 
+    # set WIFI
+    SET_SSID_PWD = 0xB0
+    GET_SSID_PWD = 0xB1
+    SET_SERVER_PORT = 0xB2
+
+    # Get the measured distance
+    GET_TOF_DISTANCE = 0xC0
+
 
 class DataProcessor(object):
     # Functional approach
@@ -138,9 +146,11 @@ def _is_frame_header(self, data, pos1, pos2):
         return data[pos1] == ProtocolCode.HEADER and data[pos2] == ProtocolCode.HEADER
 
     def _process_received(self, data, genre):
+        if genre == 177:
+            data = str(data)[2:-1].split(": ")
+            return data[1][0:-9], data[-1]
         if not data:
             return []
-        # print(data)
         data = bytearray(data)
         data_len = len(data)
         # Get valid header: 0xfe0xfe
@@ -186,8 +196,23 @@ def _process_single(self, data):
 
 def write(self, command, method=None):
     if method == "socket":
+        if command[3] == 176 and len(command) > 5:
+            command = "'"+command[4]+"'"+"("+command[5]+")"
+            command = command.encode()
         self.sock.sendall(bytes(command))
-        data = self.sock.recv(1024)
+        if command[-2] == 177:
+            while True:
+                data = self.sock.recv(1024)
+                if b'password' in data:
+                    break
+        if command[-2] == 192:
+            data = b''
+            while True:
+                data += self.sock.recv(1024)
+                if len(data) == 6:
+                    break
+        else:
+            data = self.sock.recv(1024)
         return data
     else:
         self.log.debug("_write: {}".format(command))

pymycobot/generate.py

@@ -105,6 +105,10 @@ def _mesg(self, genre, *args, **kwargs):
         """
         command_data = self._process_data_command(args)
 
+        if genre == 178:
+            # 修改wifi端口
+            command_data = self._encode_int16(command_data)
+
         LEN = len(command_data) + 2
         command = [
             ProtocolCode.HEADER,
@@ -117,7 +121,6 @@ def _mesg(self, genre, *args, **kwargs):
 
         real_command = self._flatten(command)
         has_reply = kwargs.get("has_reply", False)
-
         return real_command, has_reply
 
     # System status
@@ -525,3 +528,36 @@ def get_basic_input(self, pin_no):
             pin_no: pin port number.
         """
         return self._mesg(ProtocolCode.GET_BASIC_INPUT, pin_no, has_reply=True)
+
+    def set_ssid_pwd(self, account, password):
+        """Set connected wifi account and password. (Apply to m5 or seeed)
+
+        Args:
+            return: (account, password)
+        """
+        self._mesg(ProtocolCode.SET_SSID_PWD)
+        return self._mesg(ProtocolCode.SET_SSID_PWD, account, password)
+
+    def get_ssid_pwd(self):
+        """Get connected wifi account and password. (Apply to m5 or seeed)
+
+        Args:
+            return: (account, password)
+        """
+        return self._mesg(ProtocolCode.GET_SSID_PWD, has_reply=True)
+
+    def set_server_port(self, port):
+        """Set server port. (Apply to m5 or seeed)
+
+        Args:
+            port: Set server port.
+        """
+        return self._mesg(ProtocolCode.SET_SERVER_PORT, port)
+
+    def get_tof_distance(self):
+        """Get the measured distance.
+
+        Args:
+            return: The unit is mm
+        """
+        return self._mesg(ProtocolCode.GET_TOF_DISTANCE)

pymycobot/mycobot.py

@@ -95,6 +95,7 @@ def _mesg(self, genre, *args, **kwargs):
                 ProtocolCode.GET_SPEED,
                 ProtocolCode.GET_ENCODER,
                 ProtocolCode.GET_BASIC_INPUT,
+                ProtocolCode.GET_TOF_DISTANCE
             ]:
                 return self._process_single(res)
             elif genre in [ProtocolCode.GET_ANGLES]:

pymycobot/mycobotsocket.py

@@ -103,6 +103,7 @@ def _mesg(self, genre, *args, **kwargs):
                 ProtocolCode.GET_SPEED,
                 ProtocolCode.GET_ENCODER,
                 ProtocolCode.GET_BASIC_INPUT,
+                ProtocolCode.GET_TOF_DISTANCE
             ]:
                 return self._process_single(res)
             elif genre in [ProtocolCode.GET_ANGLES]:

pymycobot/mypalletizer.py

@@ -137,6 +137,7 @@ def _mesg(self, genre, *args, **kwargs):
                 ProtocolCode.GET_SPEED,
                 ProtocolCode.GET_ENCODER,
                 ProtocolCode.GET_BASIC_INPUT,
+                ProtocolCode.GET_TOF_DISTANCE
             ]:
                 return self._process_single(res)
             elif genre in [ProtocolCode.GET_ANGLES]: