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():
nav.setup()
print 'Starting gate'
sched.sleep(1)
nav.depth(1)
sched.sleep(.1)
nav.depth(1) # In case of skip first waypoint bug?
# Maybe set heading
nav.fd(11)
return True
def maintask():
print 'Waiting for nav setup'
nav.setup()
print 'Beginning in 2 seconds'
sched.sleep(2)
print 'Strafe square'
nav.depth(1)
nav.fd(2)
nav.rstrafe(2)
nav.bk(2)
nav.lstrafe(2)
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 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 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(practice):
nav.setup()
with mission.State('hydro1'):
if not hydrophone.run(practice):
return False
with mission.State('wreath_grab'):
if not wreath_grab.run():
print 'Failed to grab'
occluded_sel.setup()
sched.sleep(.5)
if occluded_sel.get_object() != None:
print 'Vision occluded'
occluded = True
else:
occluded = False
if practice:
print 'Practice mission, dropping'
sub.Grabber.open()
return True
with mission.State('hydro2'):
if not hydrophone.run(practice):
return False
if not occluded:
with mission.State('drop'):
wreath_drop.run()
else:
print 'Occluded open loop drop'
nav.fd(2)
nav.depth(2)
sub.Grabber.open()
return True
def main():
print 'Search pattern test'
print 'Waiting for sub unkilled'
sub.wait_unkilled()
nav.setup()
print 'Running'
log = logger.Logger(20)
sched.sleep(.2)
nav.depth(1) # We still somehow lose the first waypoint....
sched.sleep(.2)
nav.depth(1)
sched.Task('nn_enable', enable_nn_delayed)
for i in xrange(PASSES):
print 'Beginning pass %d/%d' % (i + 1, PASSES)
nav.fd(FD_DIST)
nav.rturn(90)
nav.fd(SIDE_DIST)
nav.rturn(90)
nav.fd(FD_DIST)
nav.lturn(90)
nav.fd(SIDE_DIST)
nav.lturn(90)
print 'Surfacing'
nav.depth(.1)
log.stop()
print 'Killing'
sub.kill()
sched.sleep(.5)
print 'Done'
while True:
sched.sleep(1)
def bump():
print 'Bump'
nav.fd(2)
nav.bk(1)
