|
| 1 | +#!/usr/bin/env python3 |
| 2 | +# coding:utf-8 |
| 3 | +import socket |
| 4 | +import serial |
| 5 | +import time |
| 6 | +import logging |
| 7 | +import logging.handlers |
| 8 | +import re |
| 9 | +import fcntl |
| 10 | +import struct |
| 11 | +import traceback |
| 12 | +import RPi.GPIO as GPIO |
| 13 | + |
| 14 | +""" |
| 15 | +Instructions for use: |
| 16 | +
|
| 17 | +Please update pymycobot to the latest version before use. |
| 18 | +
|
| 19 | +`pip install pymycobot --upgrade` |
| 20 | +
|
| 21 | +Please change the parameters passed in the last line of the Server.py file, MyPalletizerServer, based on your model. |
| 22 | +
|
| 23 | +
|
| 24 | +The default model is the 260PI. |
| 25 | +
|
| 26 | + The default parameters are: |
| 27 | +
|
| 28 | + serial_num: /dev/ttyAMA0 |
| 29 | +
|
| 30 | + baud: 1000000 |
| 31 | +
|
| 32 | +
|
| 33 | +""" |
| 34 | + |
| 35 | +has_return = [0x01,0x02,0x03,0x04,0x09,0x12, 0x14, 0x15, 0x17,0x1B, 0x20,0x23, 0x27, 0x2A,0x2B,0x2D,0x2E, 0x3B,0x3D, 0x40,0x42,0x43,0x44,0x4A, 0x4B,0x50,0x51,0x53,0x62,0x65,0x69,0x90,0x91,0x92,0xC0, 0xC3,0x82,0x84,0x86,0x88,0x8A,0xD0,0xD1,0xD5,0xE1,0xE2,0xE3,0xE4,0xE5,0XE6, 0xB0] |
| 36 | + |
| 37 | +def get_logger(name): |
| 38 | + logger = logging.getLogger(name) |
| 39 | + logger.setLevel(logging.DEBUG) |
| 40 | + |
| 41 | + LOG_FORMAT = "%(asctime)s - %(levelname)s - %(message)s" |
| 42 | + #DATE_FORMAT = "%m/%d/%Y %H:%M:%S %p" |
| 43 | + |
| 44 | + formatter = logging.Formatter(LOG_FORMAT) |
| 45 | + console = logging.StreamHandler() |
| 46 | + console.setFormatter(formatter) |
| 47 | + |
| 48 | + save = logging.handlers.RotatingFileHandler( |
| 49 | + "server.log", maxBytes=10485760, backupCount=1) |
| 50 | + save.setFormatter(formatter) |
| 51 | + |
| 52 | + logger.addHandler(save) |
| 53 | + logger.addHandler(console) |
| 54 | + return logger |
| 55 | + |
| 56 | + |
| 57 | +class MyPalletizerServer(object): |
| 58 | + |
| 59 | + def __init__(self, host, port, serial_num = "/dev/ttyAMA0", baud = 1000000): |
| 60 | + """Server class |
| 61 | + |
| 62 | + Args: |
| 63 | + host: server ip address. |
| 64 | + port: server port. |
| 65 | + serial_num: serial number of the robot.The default is /dev/ttyAMA0. |
| 66 | + baud: baud rate of the serial port.The default is 1000000. |
| 67 | +
|
| 68 | + """ |
| 69 | + try: |
| 70 | + GPIO.setwarnings(False) |
| 71 | + except: |
| 72 | + pass |
| 73 | + self.logger = get_logger("AS") |
| 74 | + self.mc = None |
| 75 | + self.serial_num = serial_num |
| 76 | + self.baud = baud |
| 77 | + self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) |
| 78 | + self.s.bind((host, port)) |
| 79 | + print("Binding succeeded!") |
| 80 | + self.s.listen(1) |
| 81 | + self.mc = serial.Serial(self.serial_num, self.baud, timeout=0.1) |
| 82 | + self.connect() |
| 83 | + |
| 84 | + def connect(self): |
| 85 | + while True: |
| 86 | + try: |
| 87 | + print("waiting connect!------------------") |
| 88 | + conn, addr = self.s.accept() |
| 89 | + while True: |
| 90 | + try: |
| 91 | + print("waiting data--------") |
| 92 | + data = conn.recv(1024) |
| 93 | + command = [] |
| 94 | + for v in data: |
| 95 | + command.append(v) |
| 96 | + if command == []: |
| 97 | + print("close disconnect!") |
| 98 | + break |
| 99 | + if self.mc.isOpen() == False: |
| 100 | + self.mc.open() |
| 101 | + else: |
| 102 | + self.logger.info("get command: {}".format([hex(v) for v in command])) |
| 103 | + #command = self.re_data_2(command) |
| 104 | + if command[3] == 170: |
| 105 | + if command[4] == 0: |
| 106 | + GPIO.setmode(GPIO.BCM) |
| 107 | + else: |
| 108 | + GPIO.setmode(GPIO.BOARD) |
| 109 | + elif command[3] == 171: |
| 110 | + if command[5]: |
| 111 | + GPIO.setup(command[4], GPIO.OUT) |
| 112 | + else: |
| 113 | + GPIO.setup(command[4], GPIO.IN) |
| 114 | + |
| 115 | + elif command[3] == 172: |
| 116 | + GPIO.output(command[4], command[5]) |
| 117 | + |
| 118 | + elif command[3] == 173: |
| 119 | + res = bytes(GPIO.input(command[4])) |
| 120 | + |
| 121 | + self.write(command) |
| 122 | + if command[3] in has_return: |
| 123 | + res = self.read(command) |
| 124 | + self.logger.info("return datas: {}".format([hex(v) for v in res])) |
| 125 | + |
| 126 | + conn.sendall(res) |
| 127 | + except Exception as e: |
| 128 | + self.logger.error(traceback.format_exc()) |
| 129 | + conn.sendall(str.encode(traceback.format_exc())) |
| 130 | + break |
| 131 | + except Exception as e: |
| 132 | + self.logger.error(traceback.format_exc()) |
| 133 | + conn.close() |
| 134 | + self.mc.close() |
| 135 | + |
| 136 | + def write(self, command): |
| 137 | + self.mc.write(command) |
| 138 | + self.mc.flush() |
| 139 | + |
| 140 | + def read(self,command): |
| 141 | + datas = b"" |
| 142 | + data_len = -1 |
| 143 | + k = 0 |
| 144 | + pre = 0 |
| 145 | + t = time.time() |
| 146 | + wait_time = 0.3 |
| 147 | + while True and time.time() - t < wait_time: |
| 148 | + data = self.mc.read() |
| 149 | + k += 1 |
| 150 | + if data_len == 1 and data == b"\xfa": |
| 151 | + datas += data |
| 152 | + if [i for i in datas] == command: |
| 153 | + datas = b'' |
| 154 | + data_len = -1 |
| 155 | + k = 0 |
| 156 | + pre = 0 |
| 157 | + continue |
| 158 | + break |
| 159 | + elif len(datas) == 2: |
| 160 | + data_len = struct.unpack("b", data)[0] |
| 161 | + datas += data |
| 162 | + elif len(datas) > 2 and data_len > 0: |
| 163 | + datas += data |
| 164 | + data_len -= 1 |
| 165 | + elif data == b"\xfe": |
| 166 | + if datas == b"": |
| 167 | + datas += data |
| 168 | + pre = k |
| 169 | + else: |
| 170 | + if k - 1 == pre: |
| 171 | + datas += data |
| 172 | + else: |
| 173 | + datas = b"\xfe" |
| 174 | + pre = k |
| 175 | + else: |
| 176 | + datas = b'' |
| 177 | + return datas |
| 178 | + |
| 179 | + def re_data_2(self, command): |
| 180 | + r2 = re.compile(r'[[](.*?)[]]') |
| 181 | + data_str = re.findall(r2, command)[0] |
| 182 | + data_list = data_str.split(",") |
| 183 | + data_list = [int(i) for i in data_list] |
| 184 | + return data_list |
| 185 | + |
| 186 | + |
| 187 | +if __name__ == "__main__": |
| 188 | + ifname = "wlan0" |
| 189 | + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) |
| 190 | + HOST = socket.inet_ntoa(fcntl.ioctl(s.fileno(), 0x8915, struct.pack('256s', bytes(ifname,encoding="utf8")))[20:24]) |
| 191 | + PORT = 9000 |
| 192 | + print("ip: {} port: {}".format(HOST, PORT)) |
| 193 | + MyPalletizerServer(HOST, PORT, "/dev/ttyAMA0", 1000000) |
0 commit comments