def run():
nav.setup()
sched.sleep(.5)
nav.depth(3)
print 'Looking for box'
with mission.State('forward'):
nav.vel(.2)
vision.wait_visible(any_box_sel, 5)
print 'Getting closer'
sched.sleep(1)
if box_sels['red'].is_visible():
firstcolor = 'red'
secondcolor = 'blue'
else:
firstcolor = 'blue'
secondcolor = 'red'
if not approach_shoot(firstcolor):
return False
with mission.State('over'):
print 'Going over window'
origdepth = nav.get_waypoint().pos.z
nav.up(1.5)
nav.fd(5)
nav.lturn(180)
nav.depth(origdepth)
if not approach_shoot(secondcolor):
return False
return True
def run(drop=True):
nav.setup()
nav.depth(.4)
with sched.Timeout(60) as t:
while True:
print 'Looking for wreath'
nav.vel(.15)
vision.wait_visible(sel, 5)
with mission.State('servo'):
if not servo(sel):
print 'Failed to servo on wreath'
continue
break
if drop:
nav.down(.6)
nav.bk(.2)
sched.sleep(.5)
sub.Grabber.open()
sched.sleep(1)
sub.Grabber.disable()
return True
def approach_shoot(color):
sel = small_sels[color]
with sched.Timeout(60) as t:
while True:
print 'Forward until small ' + color + ' seen'
nav.vel(.2)
vision.wait_visible(sel, 5)
print 'Servoing for ' + color
if not servo(sel):
continue
break
if t.activated:
print 'Timeout while looking for shooter'
return False
print 'Getting close'
nav.go(.5, .1 if color == 'blue' else -.1, -.15, rel=True, speed=.1)
print 'Shooting'
sched.sleep(2)
shooters[color].shoot()
sched.sleep(1)
print 'Backing up'
nav.bk(.5)
return True
def run():
nav.setup()
nav.depth(1)
with sched.Timeout(60) as t:
while True:
print 'Looking for board'
nav.vel(.2)
vision.wait_visible(board_sel, 5)
sched.sleep(1)
print 'Servoing to board'
if not board_servo(board_sel):
print 'Failed to servo to board'
continue
break
if t.activated:
print 'Timeout on grape board'
return False
print 'Open loop grape approach'
nav.go(x=1, y=-.1, z=.2, rel=True)
push_retry(push_horizontal, 2)
print 'Open loop grape approach'
nav.go(0, .4, -.3, rel=True)
push_retry(push_vertical, 1)
print 'Turning around'
nav.depth(.5)
nav.lturn(180)
def update(self, obj):
xvel = self._get_vel(obj)
Yvel = self.kY * float(obj['center'][0])
zvel = self.kz * float(obj['center'][1])
if xvel == 0 and Yvel < 0.005 and zvel < 0.005:
return True
if self.debug:
print 'xvel', xvel, 'Yvel', Yvel
nav.vel(xvel, 0, zvel, Y=Yvel)
return False
def update(self, obj):
xvel = self._get_vel(obj)
yvel = self.ky * float(obj['center'][0])
zvel = -self.kz * float(obj['center'][1])
if xvel == 0 and abs(yvel) < self.yztol and abs(zvel) < self.yztol:
return True
if self.debug:
print 'xvel', xvel, 'yvel', yvel, 'zvel', zvel
nav.vel(xvel, yvel, zvel)
return False
def allbins():
print 'Waiting for bins to be visible...'
nav.vel(.2)
vision.wait_visible(allbins_sel, 5)
if not servo(allbins_sel):
print 'Failed to servo on all'
return (False, 'servo')
obj = allbins_sel.get_object()
print 'Number of boxes', obj['number_of_boxes']
if obj is not None and int(obj['number_of_boxes']) == 4:
return (True, None)
else:
print 'Not enough boxes, strafing'
return (False, 'strafe')
def push_horizontal():
print 'Approaching horizontal grape'
nav.vel(.2)
vision.wait_visible(horiz_grape_sel, 2)
print 'Servoing on horizontal grape'
if not grape_servo(horiz_grape_sel):
print 'Failed to servo on horizontal grape'
return False
print 'Open loop'
nav.fd(.6, speed=.05)
print 'Waiting'
sched.sleep(5)
print 'Strafing'
nav.rstrafe(.2, speed=.1)
def maintask():
print 'Waiting for nav setup'
nav.setup()
print 'Beginning in 2 seconds'
sched.sleep(2)
print 'Running'
print 'Starting waypoint', nav.get_waypoint()
nav.depth(1)
t = 0
dt = .2
basepoint = nav.get_waypoint()
while True:
vel = .3 * math.sin(2 * math.pi * t / 25.0)
nav.vel(0, vel)
sched.sleep(dt)
t += dt
def push_vertical():
print 'Approaching vertical grape'
nav.vel(.2)
vision.wait_visible(vert_grape_sel, 2)
print 'Servoing on vertical grape'
if not grape_servo(vert_grape_sel):
print 'Failed to servo on vertical grape'
return False
print 'Open loop'
nav.down(.1, speed=.1)
nav.fd(.6, speed=.05)
print 'Waiting'
sched.sleep(5)
print 'Going up'
nav.up(.2, speed=.1)
def panForBuoy(name):
print 'Panning right for ' + name
with sched.Timeout(3) as timeout1:
nav.vel(Y=.05)
vision.wait_visible(buoy_sels[name])
if not timeout1:
nav.stop(wait=False)
return True
print 'Panning left for ' + name
with sched.Timeout(6) as timeout2:
nav.vel(Y=-.05)
objs = vision.wait_visible(buoy_sels[name])
if not timeout2:
nav.stop(wait=False)
return True
print 'Pan failed for ' + name
return False
def run():
nav.setup()
nav.depth(.4)
with sched.Timeout(60) as t:
while True:
print 'Looking for wreath'
nav.vel(.2)
vision.wait_visible(sel, 5)
print 'Beginning grabs'
if not repeat_grabs():
continue
break
if t.activated:
print 'Timeout while finding wreath'
return False
return True
def run(single):
nav.setup()
nav.depth(2)
with sched.Timeout(60) as t:
with mission.State('forward'):
print 'Forward until buoy seen'
nav.vel(.2)
vision.wait_visible(buoy_sel_any, 5)
if t.activated:
print 'Timeout while looking for buoy'
return False
start = nav.get_trajectory().pos
if findBuoy(FIRST_BUOY if not single else 'red'):
bump()
else:
print 'Failed to find first buoy'
nav.bk(1)
if not single:
nav.point_shoot(*start.xyz)
print 'setting heading'
nav.heading(rad=start.Y)
print 'done heading'
if findBuoy(SECOND_BUOY):
bump()
else:
print 'Failed to find second buoy'
nav.bk(1)
print 'Going over buoys'
nav.depth(.5)
nav.heading(rad=start.Y)
nav.fd(3)
if single:
nav.rstrafe(1)
else:
nav.lstrafe(1)
return True
def run():
nav.setup()
nav.depth(2)
with sched.Timeout(1 * 40) as t:
while True:
print 'Looking for hedge'
nav.vel(.3)
vision.wait_visible(hedge_sel)
print 'See hedge!'
with mission.State('servo'):
if servo(hedge_sel):
break
if t.activated:
print 'Failed to find hedge'
return False
print 'Going through hedge'
nav.fd(6)
return True
def run(name):
nav.setup()
nav.depth(.6)
while True:
with sched.Timeout(20) as timeout:
print 'Looking for ' + name + ' pipe'
nav.vel(.2)
vision.wait_visible(pipe_sels[name])
print 'See pipe!'
if timeout.activated:
print 'Timed out on pipe'
return False
with mission.State('servo'):
if servo(pipe_sels[name]):
break
print 'Saved last pipe position'
mission.missiondata['last-pipe'] = nav.get_trajectory()
return True