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