-
Notifications
You must be signed in to change notification settings - Fork 0
/
IOInterface.py
412 lines (368 loc) · 15.9 KB
/
IOInterface.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
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
"""
This file is part of Team Brocket Robotics, licensed under the MIT License.
A copy of the MIT License can be found in LICENSE.txt
"""
import time
import math
import logging
import threading
from motor_controller import MotorController
from vision_controller import VisionController, MarkerFilter, Marker, Token
from sr import MARKER_ROBOT
from servo_controller import ServoController
from ruggeduino_controller import RuggeduinoController
from threads import *
from state_utils import StateInterrupt
UP, DOWN, MIDDLE = ('UP', 'DOWN', 'MIDDLE')
OPEN, CLOSE = ('OPEN', 'CLOSE')
CENTER, WHEEL = ('center', 'wheel')
WHEEL_SPAN = 0.378
class IOInterface(object):
def __init__(self, Robot):
self.log = logging.getLogger('Robot.IO')
self.log.info("Initializing")
# Aliases for English readability
self.drive = self.turn = self._move
try:
self._setup_controllers(Robot)
except:
self.log.critical("Could not set-up hardware controller")
raise
try:
self.running = True
tr = ThreadRegister(self)
#self.bump_thread = BumpThread()
self._bump_callback = lambda s: None
self._marker_callback = lambda m: None
#tr.add_thread(self.bump_thread)
#self.bump_thread.start()
except Exception as e:
self.log.critical("Unable to start threads")
self.log.exception(e)
raise
self.log.info("Done")
def _setup_controllers(self, robot):
self.log.debug("Setup controllers")
self.left_wheel = MotorController(robot, type='wheel', id='LEFT')
self.right_wheel = MotorController(robot, type='wheel', id='RIGHT')
self.camera = VisionController(robot)
self.grabber = ServoController(robot, type='grabber')
self.arm = ServoController(robot, type='arm')
self._setup_switch_sources(robot)
#self.bump_FrL = RuggeduinoController(robot, type='bump', id='FRONT_L')
#self.bump_FrR = RuggeduinoController(robot, type='bump', id='FRONT_R')
#self.bump_BkL = RuggeduinoController(robot, type='bump', id='BACK_L')
#self.bump_BkR = RuggeduinoController(robot, type='bump', id='BACK_R')
self.token_sensL = RuggeduinoController(robot, type='sensor', id='TOKEN_L')
self.token_sensR = RuggeduinoController(robot, type='sensor', id='TOKEN_R')
def _setup_switch_sources(self, robot):
#for id in ['FRONT_L', 'FRONT_R', 'BACK_L', 'BACK_R']:
# RuggeduinoController(robot, type='bump_src', id=id).write(False)
RuggeduinoController(robot, type='sensor_src', id='TOKEN_L').write(False)
RuggeduinoController(robot, type='sensor_src', id='TOKEN_R').write(False)
def _move(self, instruction):
if not isinstance(instruction, MoveInstruction):
raise TypeError("Invalid movement instruction")
self.log.debug("Do movement %s", instruction)
instruction.action(self)
def goto_marker(self, marker, speed, comparator=None, offset=0):
self.log.debug("goto marker %s %.1f", marker, speed)
if comparator is None or not hasattr(comparator, '__call__'):
comparator = lambda a, b: a.info.code == b.info.code
reached = False
blind_spot = 0.66
while True: # Alternatively, on each new find, call goto_marker within
horiz_dist = Marker(marker).horizontal_dist - offset
travel_dist = horiz_dist / 2 # set dist to travel half the measured
if marker.rot_y < 0:
Dir = Left # negative rot_y indicates a left turn
else:
Dir = Right # positive rot_y means right turn
if not reached: # if reached flag is true then should not rotate
degree = marker.rot_y
if degree < 1: degree += 0.7 # rotation < 1 is too subtle
if degree < 2: degree *= 2 # amplify rotation
self.turn(Dir(degree, Speed(speed / 3))) # face marker
self.drive(Distance(travel_dist, speed)) # drive half way
if reached: # set below
self.log.info("Reached marker")
break
potential_markers = [] # new markers of the same type stored here
for new_marker in self.get_markers():
if comparator(new_marker, marker):
potential_markers.append(new_marker) # append based on comparator
closest = self.get_closest_marker(potential_markers) # closest new
if closest is None: # there are no markers found
if travel_dist < blind_spot: # we have traveled beyond blind_spot
self.log.info("Distance to go in blind_spot (%.4f)",
travel_dist)
reached = True # break on next loop
else:
self.log.warning("Lost the marker")
break # lost the marker
else:
self.log.debug("Re-found marker")
c = Marker(closest)
expected = horiz_dist - min(0.1, travel_dist / 3.0)
if c.horizontal_dist >= expected:
self.log.warning("Did not travel far enough (%f >= %f) (%f, %f)" % (
c.horizontal_dist, expected, closest.dist, marker.dist))
return 0
marker = closest # the new marker to target
if not reached:
if marker.info.marker_type in MarkerFilter.include("TOKENS"):
reached = self.is_holding_token()
self.log.debug("Not reached, holding:%s", reached)
if reached: break
return reached
def face_marker(self, marker, speed):
self.log.debug("Going to face marker %s", marker)
m_orient = abs(marker.orientation.rot_y)
if m_orient < 0:
Dir1, Dir2 = Left, Right
self.log.debug("Going left then right")
else:
Dir1, Dir2 = Right, Left
self.log.debug("Going right then left")
self.turn(Dir1(abs(marker.rot_y), Speed(speed)))
self.drive(Distance((0.5 * marker.dist) /
abs(math.cos(math.radians(m_orient))), speed))
self.turn(Dir2(3 * m_orient, Speed(speed)))
def face_marker_2(self, marker, speed):
theta = abs(marker.orientation.rot_y)
length = marker.dist
if theta < 0:
Dir1, Dir2 = Left, Right
self.log.debug("Going left then right")
else:
Dir1, Dir2 = Right, Left
self.log.debug("Going right then left")
self.turn(Dir1(2 * theta, Speed(speed)))
x = (length / 2) * math.cos(math.radians(theta))
self.drive(Distance(x, speed))
angle = 2 * theta
self.turn(Dir2(angle, Speed(speed)))
self.drive(Distance(x, speed))
def face_marker_3(self, marker, speed):
self.log.debug("Going to face marker %s", marker)
m_orient = abs(marker.orientation.rot_y)
if m_orient < 0:
Dir1, Dir2 = Left, Right
self.log.debug("Going left then right")
else:
Dir1, Dir2 = Right, Left
self.log.debug("Going right then left")
self.turn(Dir1(abs(marker.rot_y), Speed(speed)))
self.drive(Distance((0.7 * marker.dist) /
abs(math.cos(math.radians(m_orient))), speed))
self.turn(Dir2(2 * m_orient, Speed(speed)))
def navigate_to_marker(self, marker, speed, comparator=None):
self.log.debug("navigating to marker")
self.face_marker_3(marker, speed) # face the marker
markers = self.get_markers() # rescan for the marker
old_marker = marker
for new_marker in markers:
if new_marker.info.code == marker.info.code:
marker = new_marker
self.log.debug("Found marker, continuing")
break
if marker is old_marker: # the marker got lost
self.log.warning("Could not find marker")
return False
if self.goto_marker(marker, speed, comparator):
return True
markers = self.get_markers() # rescan for the marker
for new_marker in markers:
if new_marker.info.code == marker.info.code:
marker = new_marker
self.log.debug("Found marker again, continuing")
break
if marker is old_marker: # the marker got lost
self.log.warning("Could not find marker")
return False
self.navigate_to_marker(marker, speed, comparator)
def is_holding_token(self):
return self.token_sensL.read() or self.token_sensR.read()
def move_arm(self, direction, delay=False):
if direction not in [UP, DOWN, MIDDLE]:
raise TypeError("Invalid arm movement")
if direction == UP:
angle = self.arm.MIN
elif direction == DOWN:
angle = self.arm.MAX
elif direction == MIDDLE:
angle = self.arm.MAX / 2
self.log.debug("Move arm %s (angle = %d)", direction, angle)
self.arm.set_angle(angle)
if delay:
self.wait(0.5)
def open_grabber(self, delay=False):
self.log.debug("OPEN grabber (angle = %d)", self.grabber.MIN)
self.grabber.set_angle(self.grabber.MIN)
if delay:
self.wait(0.7)
def close_grabber(self, delay=False):
self.log.debug("CLOSE grabber (angle = %d)", self.grabber.MAX)
self.grabber.set_angle(self.grabber.MAX)
if delay:
self.wait(0.7)
def get_markers(self, _filter=None):
markers = self.camera.find_markers()
self._marker_callback(markers)
self.log.debug("get_markers %s", str(self.camera.fmt_markers(markers)))
if _filter is not None:
markers = filter(lambda m: m.info.marker_type in _filter, markers)
return markers
def get_closest_marker(self, markers):
self.log.debug("Get closest marker to robot")
return self.camera.get_closest(markers)
def get_closest_marker_rotation(self, markers, degree=0):
self.log.debug("Get closest marker from degree (%f)", degree)
return self.camera.get_rotation_nearest(markers, degree)
def _stop_operation(self, filter=None):
self.log.info("Stopping current action")
try:
self.right_wheel.stop()
self.left_wheel.stop()
except Exception as e:
self.log.exception(e)
raise StateInterrupt('stop', 'operation.stop')
def wait(self, seconds):
self.log.debug("Wait %.4f seconds", seconds)
time.sleep(seconds)
def set_bump_handler(self, callback):
self._bump_callback = callback
def set_marker_handler(self, callback):
self._marker_callback = callback
def bumped(self, sensor):
if sensor is self.bump_FrL:
self._bump_callback('Front.Left')
elif sensor is self.bump_FrR:
self._bump_callback('Front.Right')
elif sensor is self.bump_BkL:
self._bump_callback('Back.Left')
elif sensor is self.bump_BkR:
self._bump_callback('Back.Right')
def create_token_obj(self, markers):
return Token(markers)
def stop(self):
self.log.info("Stopping all communications")
self.running = False
class MoveInstruction(object):
def __init__(self, *args):
name = self.__class__.__name__
if not hasattr(self, 'setup'):
raise TypeError("%s must be properly sub classed" % name)
self.reverse = False
self.speed = None
self.setup(*args)
self._repr = "<IO.%s%s>" % (name, str(args))
def __repr__(self):
return self._repr
def drive_motors(self, io):
if self.reverse:
io.left_wheel.backward(self.speed)
io.right_wheel.backward(self.speed - 3)
else:
io.left_wheel.forward(self.speed - 3)
io.right_wheel.forward(self.speed)
def stop_motors(self, io):
io.left_wheel.stop()
io.right_wheel.stop()
class Distance(MoveInstruction):
def setup(self, meters, speed, stop=True):
if meters < 0:
self.reverse = True
meters = -meters
else:
self.reverse = False
self.meters = meters
self.speed = speed
self.stop = stop
def action(self, io):
self.drive_motors(io)
time1 = io.left_wheel.calc_wait_time(self.meters, self.speed)
time2 = io.right_wheel.calc_wait_time(self.meters, self.speed)
io.wait(max(time1, time2))
#if time1 > time2: # if left wheel takes longer than right wheel
# io.right_wheel.stop()
#elif time2 > time1: # if right wheel takes longer than left wheel
# io.left_wheel.stop()
#io.wait(abs(time1 - time2))
if self.stop:
self.stop_motors(io)
class Time(MoveInstruction):
def setup(self, seconds, speed=None, stop=True):
self.seconds = seconds
self.speed = speed
if speed is not None:
if speed < 0:
self.reverse = True
speed = -speed
else:
self.reverse = False
self.stop = stop
def action(self, io):
self.drive_motors(io)
io.wait(self.seconds)
if self.stop:
self.stop_motors(io)
class Speed:
def __init__(self, speed):
self.speed = speed
def __repr__(self):
return "<IO.Speed(%d)>" % self.speed
class Rotation(MoveInstruction):
def __init__(self, degree, measure, pivot=CENTER, stop=True):
self.degree = degree = abs(degree)
self.left_wheel_action = None
self.right_wheel_action = None
self.stop = stop
super(Rotation, self).__init__(measure, pivot)
self._repr = "<IO.%s%s>" % (self.__class__.__name__, (degree, measure,
pivot))
self.dist = self.calc_dist(pivot)
if isinstance(measure, Time):
self.speed = (self.dist / measure.seconds) * 100.0
self.time_func = lambda io: (measure.seconds, measure.seconds)
elif isinstance(measure, Speed):
self.speed = measure.speed
self.time_func = lambda io: (io.left_wheel.calc_wait_time(self.dist,
self.speed), io.right_wheel.calc_wait_time(self.dist, self.speed))
if self.speed < 0:
self.left_wheel_action = 'backward'
self.right_wheel_action = 'backward'
else:
raise TypeError("Invalid measurement %s" % measure)
def action(self, io):
if self.left_wheel_action is not None:
getattr(io.left_wheel, self.left_wheel_action)(self.speed)
if self.right_wheel_action is not None:
getattr(io.right_wheel, self.right_wheel_action)(self.speed)
time1, time2 = self.time_func(io)
io.wait(min(time1, time2))
if time1 > time2: # if left wheel takes longer than right wheel
io.right_wheel.stop()
elif time2 > time1: # if right wheel takes longer than left wheel
io.left_wheel.stop()
io.wait(abs(time1 - time2))
self.stop_motors(io)
def calc_dist(self, point):
"""Returns the distance the driving wheel needs to turn given the point
of rotation (WHEEL or CENTER)."""
var1 = math.pi * WHEEL_SPAN * abs(self.degree)
if point == WHEEL:
var1 *= 2
return var1 / 360
class Right(Rotation):
def setup(self, instruction, pivot):
self.left_wheel_action = 'forward'
self.right_wheel_action = 'backward'
if pivot == WHEEL:
self.right_wheel_action = None
class Left(Rotation):
def setup(self, instruction, pivot):
self.left_wheel_action = 'backward'
self.right_wheel_action = 'forward'
if pivot == WHEEL:
self.left_wheel_action = None