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 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 run_hedge_fix_right():
nav.setup()
nav.depth(2)
nav.set_waypoint_rel(nav.make_waypoint(y=1.5, x=-.5, Y=math.radians(-10)))
nav.wait()
# nav.rstrafe(1)
# nav.lturn(20)
return True
def run_hedge_fix_left():
nav.setup()
nav.depth(2)
nav.set_waypoint_rel(nav.make_waypoint(y=-1, x=-.5, Y=math.radians(10)))
nav.wait()
# nav.lstrafe(1)
# nav.rturn(20)
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 run():
nav.setup()
nav.depth(.4)
if 'last-pipe' in mission.missiondata:
pos = mission.missiondata['last-pipe']
pos = pos.relative_from(nav.make_waypoint(x=-2))
print 'Returning to last pipe position: ', pos
nav.point_shoot(pos)
nav.heading(rad=pos.Y)
else:
print 'No last pipe saved!'
return True
def run():
nav.setup()
nav.depth(1.5)
sched.sleep(1)
nav.do_a_barrel_roll()
print 'barrel roll done'
sched.sleep(1)
print 'killed'
sub.kill()
return True
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 run(practice):
nav.setup()
print 'Going to .4 depth'
nav.depth(.4)
print 'Waiting for hydrophone ping'
donepings = 0
while donepings < 3:
sub.Hydrophones.wait()
practice_delta = abs(sub.Hydrophones.frequency - practice_freq)
if practice:
if practice_delta > freq_tol:
print 'Competition ping', sub.Hydrophones.frequency
continue
else:
if practice_delta < freq_tol:
print 'Practice ping', sub.Hydrophones.frequency
continue
if sub.Hydrophones.declination > math.radians(50):
donepings += 1
else:
donepings = 0
speed = 1.2 - sub.Hydrophones.declination
if speed > .8:
speed = .8
elif speed < .1:
speed = .1
Y = .6 * sub.Hydrophones.heading
if abs(Y) > math.radians(30):
speed = 0
xvel = speed*math.cos(Y)
yvel = speed*math.sin(Y)
print 'Y ' + str(math.degrees(Y)) + ' speed ' + str(speed) + ' declination ' + str(math.degrees(sub.Hydrophones.declination))
nav.set_waypoint_rel(nav.make_waypoint(velx=xvel, vely=yvel, Y=Y))
print 'Done'
nav.stop()
return True
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():
nav.setup()
print 'Going to depth'
nav.depth(.2)
with sched.Timeout(90) as t:
left = True
while True:
(ok, failtype) = allbins()
if ok:
break
if failtype == 'strafe':
# t.cancel()
if left:
nav.lstrafe(.25)
else:
nav.rstrafe(.25)
left = not left
if t.activated:
print 'Timed out looking for all bins'
return False
nav.down(.2)
center_pos = nav.get_trajectory().pos
print 'Center Pos:', center_pos
dropball(bin1_sel)
print 'Returning to center...'
nav.set_waypoint(nav.Waypoint(center_pos), speed=.3)
nav.wait()
sched.sleep(1.5)
print 'Done!'
dropball(bin2_sel)
print 'Returning to center...'
waypoint = nav.Waypoint(center_pos)
waypoint.pos.z = 2.5
nav.set_waypoint(waypoint)
nav.wait()
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(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
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 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 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
