-
Notifications
You must be signed in to change notification settings - Fork 1
/
robot_commu.py
executable file
·155 lines (131 loc) · 5.37 KB
/
robot_commu.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
#!/usr/bin/env python
#-*- coding:utf-8 -*-
from subprocess import Popen, PIPE
import os, os.path
import sys
import time
from serial import Serial
from cube_capture import CubeCapture
from cube_def import *
# command code
START_CAPTURE = 0x10
FINISH_CAPTURE = 0x1f
START_SOLVING = 0x20
MANEUVER = 0x70
FINISH_MANEUVER = 0x7F
class SolverRobot(object):
def __init__(self, port=None, without_robot=False, dump_file="serial.log"):
self.without_robot = without_robot
self.colors_list = [] # up, front, right, back, left, down
if self.without_robot: # for debug
self.port = open(dump_file, "wb")
else:
if not port:
port = self.enum_select_device()
self.port = Serial(port, 9600, timeout=10)
print self.port
def capture_cube(self):
print "-------- starting cube_capture.py ......"
cc = CubeCapture(without_robot=self.without_robot, next_face=self)
colors_str = cc.main()
print "-------- finished cube_capture.py !"
if not colors_str:
raise Exception, "cube_capture.py failed or quit"
return colors_str
def solve(self, colors):
print "-------- starting solver.py .........."
p = Popen("jython solver.py -e -m ROBOT \"%s\"" % colors, shell=True, stdout=sys.stdout, stderr=PIPE,
cwd=os.path.join(os.getcwd(), os.path.dirname(__file__)),
close_fds=True)
retcode = p.wait()
print "-------- finished solver.py !"
solu = p.stderr.read()
if (not solu) or (retcode == 255):
raise Exception, "solver.py failed and quit"
else:
return solu
def send_command(self, code, arg=""):
packet = "$LL%c%s\n" % (chr(code), arg)
packet = packet.replace("LL", chr(len(packet)%256) + chr(len(packet)/256))
print "--------\n" + "".join(["\\x%02x" % ord(c) for c in packet]) + "\n--------"
self.port.write(packet)
if self.without_robot: # for debug
print "[*] called send_command(0x%x, %s)" % (code, arg)
print "[*] writing to the dump file..."
return len(packet)
#print self.port
#recv = self.port.readline()
#print "".join(["\\x%02x" % ord(c) for c in recv])
#if recv == "#\x00\x04%c\n" % chr(code+0x80):
# return len(packet)
#else:
# raise IOError, "Robot didn't reply correct data!"
def recv_command(self, code):
if self.without_robot:
print "[*] called recv_command(0x%x)" % (code)
return
#recv = self.port.read(5)
#if recv == "#\x00\x04%c\n" % chr(code+0x80):
# packet = "$LL%c\n" % chr(code)
# packet = packet.replace("LL", chr(len(packet)%256) + chr(len(packet)/256))
# self.port.write(packet)
# return len(recv)
#else:
# raise IOError, "Robot sent unexpected data!"
def main(self):
self.send_command(START_CAPTURE)
time.sleep(1) ##
colors = self.capture_cube()
self.send_command(FINISH_CAPTURE)
time.sleep(1) ##
print "captured colors:", colors
r = self.solve(colors)
self.send_command(START_SOLVING)
time.sleep(1)
self.send_command(MANEUVER, r)
#self.recv_command(FINISH_MANEUVER)
self.port.close()
return
def enum_select_device(self):
devlist = []
counter = 1
for dname in os.listdir("/dev/"):
if dname.startswith("tty."):
devlist.append("/dev/"+dname)
print "%d. /dev/%s" % (counter, dname)
counter += 1
dnum_str = raw_input("device number >")
try:
dnum = int(dnum_str)
devname = devlist[dnum-1]
except:
raise ValueError, "Please enter valid device number!"
else:
return devname
if __name__ == "__main__":
from optparse import OptionParser
parser = OptionParser("Usage: ./%prog [options] [DEVICE_NAME]")
parser.add_option("-e", "--enum_device", dest="enum_device",
action="store_true", default=False,
help="enumerate and select device name (ignore DEVICE_NAME)")
parser.add_option("-d", "--debug", dest="debug",
action="store_true", default=False,
help="enable debug mode(withoout robot)")
parser.add_option("-o", "--dump_file", dest="dump_file", metavar="FILEPATH",
action="store", default="serial.log",
help="specify dump file of serial simulation for debug(default is \"serial.log\")")
(options, args) = parser.parse_args()
port = False
if len(args) > 0:
port = args[0]
if options.enum_device:
port = None
if port == False: # not specified DEVICE_NAME
if options.debug: # for debug
robot = SolverRobot(without_robot=True, dump_file=options.dump_file)
robot.main()
else:
parser.print_help()
else: # for normal use
robot = SolverRobot(port=port)
robot.main()