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(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 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(self):
with State(self.name):
for mission in self.missions:
with State(mission.name):
with sched.Timeout(mission.timeout) as t:
ok = mission.func()
if not ok:
return (False, mission)
if t.activated:
print mission.name + ' failed due to timed out'
return (False, mission)
return (True, None)
def run(self):
print 'Waiting for sub unkilled'
sub.wait_unkilled()
print 'Running main list'
try:
with sched.Timeout(MAIN_MISSION_TIME):
(ok, failedmission) = self.main_list.run()
if not ok:
print 'Main list failed'
except greenlet.GreenletExit:
raise
except Exception, ex:
print ex
def log():
print 'Reading initial magnetometer'
sub.IMU.mag
print 'Waiting for %d seconds' % args.wait
sched.sleep(args.wait)
print "Logging"
with sched.Timeout(args.time):
with open(args.file + '.csv', 'w') as f:
while True:
sched.sleep(1.0 / 50)
try:
f.write('%.10e,%.10e,%.10e\n' % sub.IMU.mag)
except dds.Error as e:
pass
print "Done"
def run(self):
with sched.Timeout(random.random() * 10 + 5) as timeout:
while True:
if random.random() < .1:
print self.num, "yield"
sched.yield_()
elif random.random() < .1:
print self.num, "exit"
break
else:
print self.num, "sleep"
sched.sleep(4 * random.random())
if timeout.activated:
print self.num, "timed out"
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():
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