def findBucketForXSeconds(self, x):
startTime = time.time()
while time.time() - startTime < x:
print "Reading buckets"
markers = self.R.see(res=(1280, 1024)).processed()
buckets = markers.buckets
if buckets:
target = buckets[0]
drivingTo = min(target.desirableRobotTargets, key=abs)
if abs(drivingTo) < 0.2:
print "Nearly at a bucket"
angle = Bearing.ofVector(target.center)
if abs(angle) > 5:
print "Off by %s" % angle
self.R.rotateBy(angle)
print "Found bucket"
self.R.driveDistance(0.75)
self.R.drive(15)
time.sleep(1)
self.R.stop()
self.R.lifter.up()
time.sleep(1)
self.R.lifter.down()
return True
else:
print "Found no buckets"
self.R.rotateBy(30, fromTarget=True)
self.R.stop()
return False
def findBucketForXSeconds(self, x): startTime = time.time() while time.time() - startTime < x: print "Reading buckets" markers = self.R.see(res=(1280,1024)).processed() buckets = markers.buckets if buckets: target = buckets[0] drivingTo = min(target.desirableRobotTargets, key=abs) if abs(drivingTo) < 1: print "Nearly at a bucket" angle = Bearing.ofVector(target.center) if abs(angle) > 10: print "Off by %s" % angle self.R.rotateBy(angle) print "Found bucket" self.R.driveTo(target.center, speed=50) time.sleep(1) self.R.lifter.up() self.R.drive(30) time.sleep(1.5) self.R.stop() self.R.lifter.down() time.sleep(1) self.R.lifter.up() time.sleep(1) self.R.lifter.down() return True else: #One more movement self.R.driveTo(drivingTo) time.sleep(0.1) #Turn to face where we think the bucket should be, so we can see it next loop angle = Bearing.ofVector(target.center) print "Turning to face bucket again" self.R.rotateBy(angle - math.copysign(2, angle)) self.R.stop() else: print "Found no buckets" self.R.rotateBy(60, fromTarget=True) self.R.stop() return False
def main(R):
# Whether searching for cubes or a bucket
followState = "cube"
# Any cubes already found
foundCubes = set()
# Distance between each step to move towards a bucket in meters
# (e.g. if 2 meter step, every 2 meters it will check location of bucket again)
STEPDIST = 1
while True:
# Find all markers it can see
markers = R.see(res=(1280, 1024)).processed()
# If finding cubes
if followState == "cube":
print "Reading markers"
# Get only the unique (not found already) tokens
tokens = markers.tokens
# Are there any tokens?
if tokens:
if abs(tokens[0].center) > 1:
R.driveTo(tokens[0].center, gap=0.75)
else:
R.driveTo(tokens[0].center, gap=0.2)
foundCubes.add(tokens[0].id)
print "Found cube #%d!" % tokens[0].id
R.arm.grabCube(wait=True)
time.sleep(2)
R.driveDistance(-0.25)
else:
# Couldn't find any markers
print "No Marker..."
# Spin 30 degrees clockwise
R.rotateBy(30, fromTarget=True)
# Disable heading correction
R.stop()
if len(foundCubes) >= 3:
# If found 3 or more cubes, start finding bucket
followState = "bucket"
# Reset found cubes
foundCubes = set()
elif followState == "bucket":
# If finding bucket, get all seen buckets
buckets = markers.buckets
if buckets:
# If any buckets found, target first bucket in list
b = buckets[0]
# Choose the best point on the bucket to align to
target = min(b.desirableRobotTargets, key=abs)
# Print bucket information
print "Bucket seen at %s" % b.center
print "Driving to %s" % target
if abs(target) < 0.2:
# Nearly at the target, don't move any more
print "Nearly there"
angle = Bearing.ofVector(b.center)
if abs(angle) > 5:
# Need to turn a bit more to face the bucket
print "Off by %s" % angle
R.rotateBy(angle)
# Angle is ok now - go for it!
print "Success!"
R.driveDistance(0.75)
R.drive(15)
time.sleep(1)
R.stop()
R.lifter.up()
time.sleep(1)
R.lifter.down()
# Start following cubes again, skip the rest of the loop
followState = "cube"
continue
else:
if abs(target) > STEPDIST:
# Too far from target - limit movement
target = STEPDIST * target.normalize()
# One more movement
R.driveTo(target)
time.sleep(0.1)
# Turn to face where we think the bucket should be, so we can see it next loop
targetFacing = b.center - target
angleDifference = Bearing.ofVector(targetFacing) - Bearing.ofVector(target)
print "Turning to face bucket again"
R.rotateBy(angleDifference)
R.stop()
R.rotateBy(30, fromTarget=True)
R.stop()
if buckets:
# Take first bucket as target
b = buckets[0]
# Choose the best point on the bucket to align to
target = min(b.desirableRobotTargets, key=abs)
# Print information to read
print "Bucket seen at %s" % b.center
print "Driving to %s" % target
# If very close to the target
if abs(target) < 0.1:
# Print nearly there and get angle to center of the bucket
print "Nearly there"
angle = Bearing.ofVector(b.center)
# If angle is not aligned enough
if abs(angle) > 5:
# Turn a bit more to face the bucket and print how far off
print "Off by %s" % angle
R.rotateBy(angle)
# Angle is ok now - go for it!
print "Success!"
R.drive(15)
time.sleep(1)
R.stop()
time.sleep(10)
else: