| 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
|
from pyrobot.brain.behaviors import *
from time import *
class GatherPucksBrain(FSMBrain):
def setup(self):
if not self.robot.hasA("camera"):
self.startDevice('camera') # or BlobCamera, or PlayerCamera
if not self.robot.hasA("gripper"):
self.startDevice('gripper')
self.camera = self.robot.camera[0]
self.camera.clearFilters()
self.camera.addFilter("match",255,0,0,) # red
self.camera.addFilter("blobify",0,255,255,0,1,1,1,) # red
class locatePuck(State):
"""
Rotate until a red blob is seen or a complete scan has been done.
"""
def onActivate(self):
self.searches = 0 # counter for number of rotations
def step(self):
redBlobs = self.robot.camera[0].filterResults[1]
if len(redBlobs) != 0:
self.move(0, 0)
print "found a puck!"
self.goto('approachPuck')
elif self.searches > 275:
print "found all pucks"
self.goto('done')
else:
print "searching for a puck"
self.searches += 1 # update counter
self.move(0, 0.2)
class approachPuck(State):
"""
Move towards closest red blob by centering it in blobfinder.
Once centered, then consider distance to the closest red blob.
Once close, move slowly until it is within gripper.
"""
def step(self):
redBlobs = self.robot.camera[0].filterResults[1]
if len(redBlobs) == 0:
print "no puck in sight"
self.goto('locatePuck')
elif redBlobs[0][2] < 50: # sizes need to be changed for pyrobot
print "puck far left"
self.move(0, 0.2)
elif redBlobs[0][2] < 75:
print "puck to left"
self.move(0.2, 0.1)
elif redBlobs[0][2] > 100:
print "puck far right"
self.move(0, -0.2)
elif redBlobs[0][2] > 85:
print "puck to right"
self.move(0.2, -0.1)
else:
if self.robot.gripper[0].getBreakBeam('inner') > 1:
print "grabbing puck"
self.move(0, 0)
self.goto('grabPuck')
elif redBlobs[0][4] > 500: #area
print "puck straight ahead"
self.move(0.3, 0)
else:
print "puck very close"
self.move(0.1, 0)
class grabPuck(State):
"""
Pick up the current puck and then return to locating another puck.
"""
def step(self):
self.robot.gripper[0].store()
sleep(2.0)
self.robot.gripper[0].open()
sleep(1.0)
self.goto('locatePuck')
class done(State):
"""
No more visible pucks, so stop moving.
"""
def update(self):
self.move(0, 0)
def INIT(engine):
brain = GatherPucksBrain(engine=engine)
brain.add(locatePuck(1))
brain.add(approachPuck())
brain.add(grabPuck())
brain.add(done())
return brain |
