def second_pipe(grp): return Sequential( Log('Going to Search Depth'), Depth(PIPE_SEARCH_DEPTH), Retry(lambda: Sequential( Zero(), Log('Sway searching for pipe'), search_task(), Log('Centering on pipe'), center(), Depth(PIPE_FOLLOW_DEPTH), Log('Aligning with pipe'), Concurrent( align(), center(), finite=False ), ), float("inf")), Zero(), Log('Aligned, moving forward'), Timed(VelocityX(.4),3), Zero() #Depth(PIPE_FOLLOW_DEPTH) )
def one_pipe(grp): return Timeout( 45, Sequential( Log('Going to Search Depth'), Depth(PIPE_SEARCH_DEPTH), Zero(), Log('Sway searching for pipe with Behind'), TrackMovementY(search_task_behind()), Retry( lambda: Sequential( Zero(), Log('Sway searching for pipe; may have been lost'), TrackMovementY(search_task(), shm.jank_pos.y.get()), Log('Centering on pipe'), Conditional( center(), on_fail= Fail( Sequential( Log("Pipe lost, Attempting to Restore Y pos"), Zero(), TrackMovementY(RestorePosY(.3), shm.jank_pos.y.get()), ))), Depth(PIPE_FOLLOW_DEPTH), Log('Aligning with pipe'), Concurrent(align(), center(), finite=False), ), float("inf")), Zero(), Log('Aligned, moving forward'), Timed(VelocityX(.4), 3), Zero() #Depth(PIPE_FOLLOW_DEPTH) ))
def both_paths(): return Sequential(Log('Going to Search Depth'), Depth(PATH_SEARCH_DEPTH), Zero(), Log('Beginning to align to Path 1'), one_path(shm.path_results_1), Log('Moving forward'), Timed(VelocityX(.2), 1), Log('Beginning to align to Path 2'), one_path(shm.path_results_2), Log('Done'), Zero())
def DepthAlign(depth): return Concurrent( AlignTargetBin(), CenterTargetBin(1), Depth(depth), finite=False, )
def on_first_run(self, vision, *args, **kwargs): check = lambda: checkAligned(vision) self.use_task( Conditional( While( lambda: Sequential( Log("Restoring depth"), Depth(constants.WIRE_DEPTH), Log('Align Heading'), AlignHeading(vision), Zero(), Log('Align Sway'), AlignSway(vision), Zero(), Log('Align Heading'), AlignHeading(vision), Zero(), Log('Align Fore'), AlignFore(vision), Zero(), ), lambda: checkNotAligned(vision) ), on_fail=Sequential( Log('Lost the gate, backing up and looking again'), Succeed(Concurrent( Fail(Timed(VelocityX(-.4), 6)), Fail(IdentifyGate(vision)), )), Fail(), ) ) )
def Full(): return Sequential( Log('Changing depth before hydrophones'), Depth(settings.depth), OptimizablePinger(), ) full = Full()
def on_first_run(self, north, east, heading=None, depth=None, optimize=False, rough=False, deadband=0.05): self.north = north self.east = east if heading is None: self.heading = shm.navigation_desires.heading.get() else: self.heading = heading if depth is None: self.depth = shm.navigation_desires.depth.get() else: self.depth = depth self.use_task( WithPositionalControl( Concurrent(PositionN(self.north, error=deadband), PositionE(self.east, error=deadband), Heading(self.heading, error=deadband), Depth(self.depth, error=deadband)), optimize=optimize, ))
def pitch_pipe(grp): return Sequential( Depth(PIPE_SEARCH_DEPTH), pitch_search_task(), Zero(), center(), Pitch(0), center(), Depth(PIPE_FOLLOW_DEPTH), Concurrent( #center(), align(), finite=False, ), PositionalControl(), Zero(), )
def on_first_run(self, vision, amlan, *args, **kwargs): self.use_task(Zeroed(Sequential( Log('Aligning Amlan'), AlignAmlan( vision, vision.target_bin, amlan, Depth(constants.above_bin_depth), ), Log('Moving to handle grab depth'), Timed(Depth(constants.grab_depth), self.GRAB_TIME), Log('Extending Amlan'), amlan.Extend(), Timer(2), )))
def on_first_run(self, time, *args, **kwargs): self.use_task(Sequential( Depth(constants.WIRE_THRU_DEPTH, error=.1), Timed(VelocityX(.3), time), Zero(), #Timed(VelocityY(.2), 1), Zero() ))
def on_first_run(self, *args, **kwargs): self.use_task( NavigationSpeed( Sequential( Depth(recovery_constants.tower_depth, error=0.08), ConsistentTask(Heading(60 * TOWARDS_BOWL_SIGN)), MoveX(30, deadband=0.08), ), 0.4))
def on_first_run(self, *args, **kwargs): self.use_task( Sequential( Log('Going to gate depth'), Depth(constants.depth), Log('Moving through gate'), Timed(VelocityX(0.4), constants.vel_time), Zero(), ))
def on_run(self, pitch=True, roll=True): Depth(shm.kalman.depth.get())() PositionN(shm.kalman.north.get(), positional_controls=None)() PositionE(shm.kalman.east.get(), positional_controls=None)() Pitch(0)() if pitch else Pitch(shm.kalman.pitch.get())() Roll(0)() if roll else Roll(shm.kalman.roll.get())() VelocityX(0, positional_controls=None)() VelocityY(0, positional_controls=None)() self.finish()
def on_first_run(self, vision, *args, **kwargs): self.pid = PIDLoop( input_value=lambda: bbar_angle(vision), output_function=VelocityY(), target=0, deadband=3, p=0.8, d=0.4, ) self.depth = Depth(constants.WIRE_DEPTH)
def __init__(self, depth=None, *args, **kwargs): """ depth - a depth to use as the original depth """ super().__init__(*args, **kwargs) # Store the start depth of the sub if depth is None: self.start_depth = constants.BUOY_SEARCH_DEPTH else: self.start_depth = depth self.depth_task = Depth(self.start_depth, error=.01)
def search_at_depth(depth, msg="", target=(0, 0), deadband=(0.1, 0.1), depth_timeout=20): return Sequential( timed(cons(Depth(depth)), depth_timeout), cons( downward_target_task(target, deadband=deadband), success=0.80 * 2.5 * 60, total=2.5 * 60, debug=True, ), stop(), Log("Found at {} (depth={})".format(msg, depth)), )
def Full(): return Sequential( Log('Doing balls! RIP octagon'), Log('Turning away from wall'), Heading(WALL_TOWER_HEADING, error=5), Log('Surfacing'), Depth(0.5, error=0.1), Timed(Depth(0), 3), Log('Going over to press octagon'), Timed(VelocityX(0.3), 11), Log('Pulling octagon underwater'), Timed(Concurrent( VelocityX(0.3), Depth(0.5), finite=False, ), 5), Log('Snapping octagon back in place'), Timed(VelocityX(-0.2), 4), VelocityX(0), Log('Well, I guess that\'s it for balls.'), )
def on_first_run(self, *args, **kwargs): original_depth = shm.kalman.depth.get() self.use_task( WithPositionalControl( Sequential( Log('Getting close to surface'), FastDepth(0.5), Log('Surfacing!'), Timed(Depth(0), 3), Log('Falling back below surface'), FastDepth(original_depth), )))
def on_first_run(self, shm_group, *args, **kwargs): FUNNEL_DEPTH = settings.approach_funnel_depth forward_target_task = ForwardTarget( point=(shm_group.center_x.get, shm_group.center_y.get), target=norm_to_vision_forward(0.0, 0.4), depth_bounds=(.45, 1.0), deadband=norm_to_vision_forward(-0.9, -0.9), px=0.0004, py=0.0008, max_out=.05, ) heading_target_task = HeadingTarget( point=(shm_group.center_x.get, shm_group.center_y.get), target=norm_to_vision_forward(0.0, 0.4), depth_bounds=(.45, 1.0), deadband=norm_to_vision_forward(-0.9, -0.9), px=0.04, py=0.0008, max_out=(5, 0.05), ) self.use_task( Sequential( cons(Depth(FUNNEL_DEPTH)), cons( heading_target_task, debug=True ), cons( Concurrent( forward_target_task, PIDLoop( input_value=shm_group.area.get, target=9000, deadband=250, output_function=VelocityX(), reverse=True, p=0.00008, max_out=.05, ), finite=False, ), total=2.5*60, success=2.5*60*0.9, debug=True, ), ) )
def on_first_run(self, vision, *args, **kwargs): self.use_task(Conditional( Sequential( Log('Moving to depth where bins are visible'), Depth(constants.see_both_depth, error=0.2, *args, **kwargs), Log('Searching for bin'), MasterConcurrent(IdentifyBin(vision), SearchWithGlobalTimeout()), Log('Centering bins'), CenterBins(vision), ), on_fail=Fail(Log('Failed to move above bins')), ))
def DecideAndPush(): counter_wolf = VisibleCounter(shm.bins_status.wolf_visible_frames.get) counter_bat = VisibleCounter(shm.bins_status.bat_visible_frames.get) is_bat_getter = lambda: counter_wolf.get_value() < counter_bat.get_value() counter_wolf_2 = VisibleCounter(shm.bins_status.wolf_visible_frames.get) counter_bat_2 = VisibleCounter(shm.bins_status.bat_visible_frames.get) return Sequential( Depth(0.5), center_cover(), DoComparison(counter_wolf, counter_bat), FunctionTask(lambda: Log('wolf: {}'.format(counter_wolf.get_value())) ()), FunctionTask(lambda: Log('bat: {}'.format(counter_bat.get_value()))()), #MasterConcurrent( Depth(2.5), # While(center_cover, lambda: True) #), LeverWrapper(is_bat_getter=is_bat_getter, wrapped_task=PushLever()), #Conditional(Timed(FunctionTask(lambda: counter_wolf.get_value() > counter_bat.get_value()), .1), # on_success=Log("wolf visible"), # on_fail=Log("bat visible"), #), #Log('nani') Depth(.5), center_cover(), DoComparison(counter_wolf_2, counter_bat_2), FunctionTask(lambda: Log('wolf: {}'.format(counter_wolf_2.get_value())) ()), FunctionTask(lambda: Log('bat: {}'.format(counter_bat_2.get_value())) ()), Conditional( FunctionTask(lambda: decide(is_bat_getter( ), counter_wolf_2.get_value(), counter_bat_2.get_value()), on_success=full_wolf(), on_fail=full_bat())))
def __init__(self, validator, right=True, timeout=10, *args, **kwargs): """ validator - a function that returns True when a buoy is found and False otherwise. right - determines whether the submarine should move right or left during its search timeout - the amount of time to surge """ super().__init__(*args, **kwargs) # self.logv("Starting {} task".format(self.__class__.__name__)) self.validator = validator self.right = right self.surge_task = VelocityY() self.zero_task = Zero() self.depth_task = Depth(constants.BUOY_SEARCH_DEPTH) self.TIMEOUT = timeout
def on_first_run(self, *args, **kwargs): self.use_task( WithQuaternionControl( Sequential( Log('Time to celebrate!'), Depth(3, error=0.2), Pitch(90, error=5), Log('Weeeeeeeeeee'), MasterConcurrent( self.CheckDepth(), RelativeToCurrentDepth(-0.25), RelativeToCurrentHeading(10), ), FunctionTask( lambda: shm.settings_control.enabled.set(False)), Zero(), )))
def TrackerSearch(): return \ Retry( Sequential( Conditional(FunctionTask(set_second_task_if_possible), on_fail= \ Sequential( Depth(BOARD_DEPTH, error=0.2), # PingerTracker goes here Conditional(SearchBoard(), on_success=FunctionTask(lambda: set_pinger_task(Stake)), on_fail= \ Sequential( markers.set('center'), FunctionTask(lambda: set_pinger_task(Recovery)) ) ) ) ) ), attempts=3 )
def one_path(grp): return Timeout( 90, Sequential( Zero(), search_task(), While( lambda: Sequential( Zero(), Log('Centering on path'), center(grp), Log('Going to follow depth'), Depth(PATH_FOLLOW_DEPTH), Log('Aligning with path'), Heading(pathAngle(grp.get()), deadband=0.1), Zero(), Timer(1), Log(grp.angle.get()), ), lambda: checkNotAligned(grp.get())), Log('aligned'), Zero(), ))
def on_first_run(self): self.has_made_progress = True self.seen_frames_checker = ConsistencyCheck(3, 3, strict=False) def location_validator(buoy): return self.seen_frames_checker.check(buoy.probability.get() != 0) self.depth_task = DepthRestore() self.heading_task = HeadingRestore() self.up_task = DepthRestore(constants.BUOY_OVER_DEPTH) self.dodge_vel = -.4 if yellowRight else .4 self.over_task = Sequential(Timed(VelocityY(self.dodge_vel), 2), DirectionalSurge(6, .4, compensate=True)) self.heading_task = HeadingRestore() self.task = Sequential( Depth(constants.BUOY_SEARCH_DEPTH), self.heading_task, # Depth(0.9, error=.01) # SeqLog("Looking for red buoy"), LocateFirstBuoy(lambda: location_validator(red_buoy_results), forward=True, right=BUOY_RIGHT_TO_REACH[0], middle=yellow_buoy_results), Buoy(red_buoy_results, first_buoy=True, right=not redRight()), # self.depth_task, # SeqLog("Looking for green buoy stage 1"), LocateBuoyStrafe(lambda: location_validator(yellow_buoy_results), right=True, timeout=3), SeqLog("Looking for green buoy"), LocateBuoyStrafe(lambda: location_validator(green_buoy_results), right=not greenRight(), timeout=3), Buoy(green_buoy_results, right=not greenRight()), # self.depth_task, SeqLog("Looking for yellow buoy"), LocateBuoyStrafe(lambda: location_validator(yellow_buoy_results), right=not yellowRight(), timeout=2), Buoy(yellow_buoy_results, right=not yellowRight(), yellow=True), HeadingRestore(), SeqLog("Rising to Over depth"), self.up_task, SeqLog("Going around buoys"), self.over_task)
def on_run(self): if self.this_run_time - self.first_run_time > self.TIMEOUT: self.finish() # self.loge("{} timed out!".format(self.__class__.__name__)) return # self.logv("Running {}".format(self.__class__.__name__)) # if shm.kalman.depth.get() < .4: # self.giveup_task() # self.finish() if self.validator(): # if abs(shm.kalman.depth.get() - BUOY_SEARCH_DEPTH) >= BUOY_DEPTH_VARIANCE: # self.depthless_target() # else: self.target_task() else: # self.heading_task() # HeadingRestore() self.logv('lost buoy? searching and restoring depth') Depth(constants.BUOY_SEARCH_DEPTH) self.locator_task() if self.target_checker.check(self.target_task.finished): self.finish()
def TrackerSearch(): global get_pinger_task global stake return Retry(lambda: \ Sequential( Sequential( Depth(BOARD_DEPTH, error=0.2), Either( TrackPinger(), # Consistent(lambda: get_pinger_task() == stake and shm.torpedoes_stake.board_visible.get(), count=2, total=3, invert=False, result=True)), Consistent(shm.torpedoes_stake.board_visible.get, count=2, total=3, invert=False, result=True)), VelocityX(0, error=40), Log('dafuq'), Conditional(FunctionTask(set_second_task_if_possible), on_fail= \ Conditional(SearchBoard(), on_success=FunctionTask(lambda: set_pinger_task(stake)), on_fail= \ Sequential( Log('we cant see jack'), FunctionTask(lambda: set_pinger_task(surface)) ) ) ) ) ), attempts=3 )
def on_first_run(self): self.has_made_progress = True self.seen_frames_checker = ConsistencyCheck(3, 3, strict=False) def location_validator(buoy): return self.seen_frames_checker.check(buoy.probability.get() != 0) self.depth_task = DepthRestore() self.heading_task = HeadingRestore() self.up_task = DepthRestore(constants.BUOY_OVER_DEPTH) self.dodge_vel = -.4 if yellowRight else .4 self.over_task = Timed(VelocityX(.4), 8) self.heading_task = HeadingRestore() self.task = Sequential( Depth(constants.BUOY_SEARCH_DEPTH), Conditional( MasterConcurrent( Sequential( self.heading_task, # Depth(0.9, error=.01) # SeqLog("Looking for red buoy"), LocateFirstBuoy(lambda: location_validator(red_buoy_results), forward=True, right=BUOY_RIGHT_TO_REACH[0], middle=yellow_buoy_results), Buoy(red_buoy_results, first_buoy=True, right=redRight()), # self.depth_task, # SeqLog("Looking for green buoy stage 1"), LocateBuoyStrafe(lambda: location_validator(yellow_buoy_results), right=True, timeout=3), SeqLog("Looking for green buoy"), LocateBuoyStrafe( lambda: location_validator(green_buoy_results), right=greenRight(), timeout=3), Buoy(green_buoy_results, right=greenRight()), # self.depth_task, SeqLog("Looking for yellow buoy"), LocateBuoyStrafe( lambda: location_validator(yellow_buoy_results), right=yellowRight(), timeout=2), Buoy(yellow_buoy_results, right=yellowRight(), yellow=True), Log("re-aligning red buoy"), LocateBuoyStrafe( lambda: location_validator(red_buoy_results), right=secondRedRight(), timeout=2), Buoy(red_buoy_results, right=secondRedRight(), yellow=False, align_only=True), ), PreventSurfacing(), ), on_success=Sequential( Zero(), self.heading_task, SeqLog("Rising to Over depth"), self.up_task, SeqLog("Going over buoys"), self.over_task, ), on_fail=Sequential( Zero(), self.heading_task, SeqLog("Going to Over depth"), self.up_task, SeqLog("Going over buoys"), self.over_task, Timed(VelocityX(.4), 8), )))
def on_finish(self, *args, **kwargs): Depth(shm.kalman.depth.get())()