Skip to content

Commit b048dd7

Browse files
wangWkingwangWking
authored
Merge pull request #87 from elephantrobotics/mycobot_280_refactor
Mycobot 280 refactor
2 parents 8ef4b12 + 80c0577 commit b048dd7

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

46 files changed

+20273
-1926
lines changed

‎demo/Server_260.py‎

Lines changed: 193 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,193 @@
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)

‎demo/Server_270.py‎

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

0 commit comments

Comments
 (0)