def approach_right_passageway_task(): align_task = align_right_width() return FinishIf( condition=lambda: gate_elems() == 0 or (align_task.finished and align_task.success), task=Conditional( main_task=FunctionTask(lambda: gate_elems() == 1), on_success=Concurrent( focus_elem(lambda: shm.gate.leftmost_x, offset=right_offset), VelocityX(0.2)), on_fail=Conditional( main_task=FunctionTask(lambda: gate_elems() >= 2), on_success=Concurrent( # Align distance in between the right and middle poles align_task, # Align to middle of right and middle poles PIDLoop(input_value=lambda: (shm.gate.rightmost_x.get() + shm.gate.middle_x.get()) / 2 if shm.gate.rightmost_visible.get() else (shm.gate.leftmost_x.get() + shm.gate.middle_x.get( )) / 2, target=lambda: shm.gate.img_width.get() / 2, p=0.3, deadband=lambda: shm.gate.img_width.get() * 0.05, output_function=RelativeToCurrentHeading(), negate=True), finite=False, ), on_fail=NoOp(), finite=False), finite=False))
def on_first_run(self, vision, obj_func, amlan, align_depth_task, blind=False, align_during_depth=False, *args, **kwargs): def tube_angle_heading(): return shm.kalman.heading.get() + (obj_func().obs.angle % 180 - 90) self.must_see_obj = not blind def unsee(): self.must_see_obj = False self.task = Sequential( Sequential( Log('Centering object'), CenterCentroid(lambda: vision.coords([obj_func()]), precision=0), Zero(), Sequential( Log('Aligning to object'), Concurrent( CenterCentroid(lambda: vision.coords([obj_func()]), precision=0), GradualHeading(tube_angle_heading), finite=False, ), ) if not align_during_depth else NoOp(), Log('Going down and precisely aligning to object'), ConsistentTask( Concurrent( GradualHeading(tube_angle_heading), CenterCentroid(lambda: vision.coords([obj_func()]), precision=2), align_depth_task, finite=False, )), FunctionTask(unsee), ) if not blind else NoOp(), PositionalControl(), Zero(), Log('Applying Amlan offset'), amlan.CenterFromDownCam(), )
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 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 DepthAlign(depth): return Concurrent( AlignTargetBin(), CenterTargetBin(1), Depth(depth), finite=False, )
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 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 set_REMOVING(self, xp, yp, hp): self.state = State.REMOVING self.remove_start = time.time() self.old_x = xp self.old_y = yp self.old_h = hp self.removing_initial_hd = normalized_hd() # handle losing sight of the handle better # XXX will need to be tweaked, Zander is tweaking vision GUI, grabber, etc self.remove_positioning_task = Sequential( OrientTask(lambda: self.removing_initial_hd), ZeroTask(), WaitTask(3), Finite(DownwardTarget(point=self.handlef, min_out=0.1)), ZeroTask(), DescendTask(HANDLE_GRAB_DEPTH_PRE), LogTask("Descending for pre-grab..."), Concurrent(OrientTask(lambda: self.removing_initial_hd), Finite(DownwardTarget(point=self.handlef))), ZeroTask(), LogTask("Adjusting offset..."), TimedTask(RelPosTask(REMOVE_SURGE, REMOVE_SWAY), 8), # note that they are flipped here because the sub is rotated 90 when dropping/removing ZeroTask(), #DistSurgeTask(d=REMOVE_SURGE), #DistSwayTask(d=REMOVE_SWAY), DescendTask(HANDLE_GRAB_DEPTH), LogTask("grabbing handle"), ThunkTask(grab_handle), WaitTask(2), DescendTask(DISCARD_DEPTH))
def TestConcurrent(success): return LogSuccess(Concurrent( Succeed(TimedLog('1', 0.03), success), TimedLog('2', 0.04), TimedLog('3', 0.01), TimedLog('4', 0.02), ))
def on_first_run(self, vector, deadband=0.1): vector = call_if_function(vector) self.use_task( WithPositionalControl( Concurrent(PositionN(target=vector[0], error=deadband), PositionE(target=vector[1], error=deadband), finite=False)) )
def on_first_run(self, vector, deadband=0.01, *args, **kwargs): delta_north, delta_east = rotate(vector, kalman.heading.get()) n_position = RelativeToInitialPositionN(offset=delta_north, error=deadband) e_position = RelativeToInitialPositionE(offset=delta_east, error=deadband) self.use_task( WithPositionalControl( Concurrent(n_position, e_position, finite=False), ))
def DoComparison(counter_a, counter_b): return \ While( lambda: Timed(Concurrent( While(center_cover, lambda: True), counter_a, counter_b, ), 1.5), lambda: counter_a.get_value() + counter_b.get_value() < 10 )
def on_first_run(self, tasks, *args, **kwargs): #tasks.insert(0, BeginMission) tasks.append(EndMission) self.use_task(Retry(lambda: Sequential( RunTask(BeginMission), Concurrent( Sequential(subtasks=[RunTask(t) for t in tasks],), Fail(WaitForUnkill(killed=False, wait=1)), ), ), float('inf')) )
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 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 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, vision, angle, double_align=False, *args, **kwargs): def CenterTargetBin(precision): return CenterBins(vision, lambda bin: bin.id == vision.TARGET_BIN, precision=precision) def AlignTargetBin(): return AlignHeadingToAngle(lambda: vision.target_bin().obs.angle, angle, mod=180) def DepthAlign(depth): return Concurrent( AlignTargetBin(), CenterTargetBin(1), Depth(depth), finite=False, ) self.task = Zeroed(Timeout(Sequential( Log('Centering over target bin'), CenterTargetBin(0), Log('Aligning target bin'), Concurrent( AlignTargetBin(), CenterTargetBin(0), finite=False, ), Log('Going half down to precisely align with target bin'), DepthAlign((constants.see_both_depth + constants.above_bin_depth) / 2), Sequential( Log('Going down to fully align to bin'), DepthAlign(constants.above_bin_depth), ) if double_align else NoOp(), Zero(), PositionalControl(), ), self.TIMEOUT))
printer_two = lambda one, two: RunTwo(Printer(message=one), Printer(message=two)) # And then use it. printer_remix = printer_two('Hello', 'but actually...') printer_remix() # However, although RunTwo will work properly, it's not very good style. # We can easily create a task that will run any number of subtasks in parallel. # (which is a nice segue into...) ## Combinators # Concurrent runs multiple tasks at the same time. Here, again, we're binding some default arguments to the run method. # Note: This will initialize 2 separate Printer tasks. printer_twice = Concurrent(Printer("Concurrent 1"), Printer("Concurrent 2")) printer_twice() # We can also pass a list of tasks, with the "subtasks" keyword parameter. # Note: This will initialize 3 separate Printer tasks. printer_three_times = Concurrent(subtasks=[Printer(i) for i in range(3)]) printer_three_times() # Tasks can be generated in procedural ways. # Note: This will initialize 3 separate Printer tasks. printer_indeed = Concurrent(subtasks=[Printer(message=s) for s in ['Pie', 'Cake', 'American Politics']]) printer_indeed() # Sequential runs multiple tasks in sequence, but it requires tasks to implement certain methods. # Let's create a rather silly task to demonstrate.
def PushLever(): def TargetLever(py): return ForwardTarget( point=lambda: (shm.bins_status.lever_x.get(), shm.bins_status.lever_y.get() / (1 + shm.bins_status.lever_sz.get() / 50)), #target=lambda: (0, .3 * min(100, shm.bins_status.lever_sz.get()) / 100), target=lambda: (0, .25 / (1 + shm.bins_status.lever_sz.get() / 50)), valid=shm.bins_status.lever_visible.get, deadband=(DEADBAND, DEADBAND), px=1, py=py, ix=.05, iy=.05) return Sequential( Log("PushLever start"), FunctionTask(VelocityX(.2)), MasterConcurrent( Consistent(lambda: shm.bins_status.lever_sz.get() > 90, count=.5, total=.75, invert=False, result=True), While(lambda: TargetLever(1.5), lambda: True), While( lambda: FunctionTask( VelocityX(.2 / (1 + 2 * (abs(shm.bins_status.lever_x.get( )) + abs(shm.bins_status.lever_y.get() - .25))))), lambda: True)), #Log("Higher P"), #MasterConcurrent( # Consistent(lambda: shm.bins_status.lever_sz.get() > 100, count=.5, total=.75, invert=False, result=True), # While(lambda: TargetLever(.8), lambda: True), # While(lambda: FunctionTask( # VelocityX(.2 / (1 + 2 * (abs(shm.bins_status.lever_x.get()) + abs(shm.bins_status.lever_y.get()-.15)))) # ), lambda: True) #), #Log("targeting"), #TargetLever(), Log("zoom zoom"), #RelativeToInitialDepth(-.1), #Timed(RelativeToCurrentDepth(-2), .7), #FunctionTask(RelativeToInitialDepth(0)), Concurrent( Sequential( Timer(1), Log("Forwards!"), FunctionTask(VelocityX(1)), #VelocityDepth(0), ), Sequential( Timed(RelativeToCurrentDepth(-.4), 1), RelativeToInitialDepth(0), ), Timer(4), ), #Timed( # While(TargetLever, lambda: True), # 5 #), Timed(VelocityX(-.8), .5), VelocityX(0), Log("waiting"), Timer(4), #RelativeToInitialHeading(0), #Timed(VelocityX(-.8), 1), #RelativeToInitialHeading(0), #FunctionTask(VelocityX(0)), #Timer(5), #TargetLever() )
lambda: Sequential( Log("attempt asdasd"), Concurrent( DownwardTarget(lambda: (shm.path_results.center_x.get(), shm.path_results.center_y.get()), target=(lambda: heading_to_vector(heading()) * -dst), deadband=(deadband, deadband), px=1, py=1, ix=.05, iy=.05), While( lambda: Sequential( FunctionTask(lambda: shm.navigation_desires .heading.set((shm.kalman.heading.get( ) - .95 * math.degrees( heading_sub_degrees( trg, heading(), math.pi * 2))) % 360) if shm.path_results.num_lines.get( ) == 2 else None), #Log('{:03d} '.format(int(shm.navigation_desires.heading.get())) + s2(int(math.degrees(trg)), int(math.degrees(heading())), int(shm.desires.heading.get())) if shm.path_results.num_lines.get() == 2 else 'X'), Timer(.05)), lambda: abs( heading_sub_degrees(trg, heading(), math.pi * 2) ) > math.radians(5) ) #, count=6, total=8, invert=False, result=True), )), lambda: not is_done( heading, trg, dst, deadband
# TODO this might not be working print(angle_1, angle_2, diff) if self.angle_1_checker.check( diff > 0 ^ (angle_1 < angle_2) ^ (not bend_right)): self.finish() if self.angle_2_checker.check( diff < 0 ^ (angle_1 < angle_2) ^ (not bend_right)): self.finish(success=False) PipeAlign = lambda heading: Concurrent( DownwardTarget(lambda: (shm.path_results.center_x.get(), shm.path_results.center_y.get()), target=(0, -.25), deadband=(.1, .1), px=0.5, py=0.5), Log("Centered on Pipe!"), FunctionTask(lambda: shm.navigation_desires.heading.set( -180 / 3.14 * heading.get() + shm.kalman.heading.get()))) FollowPipe = lambda h1, h2: Sequential( PipeAlign(h1), Zero(), Log("Aligned To Pipe!"), DownwardTarget(lambda: (shm.path_results.center_x.get(), shm.path_results.center_y.get()), target=(0, 0), deadband=(.1, .1), px=0.5, py=0.5),
#!/usr/bin/env python3.4 from mission.framework.combinators import Sequential, Concurrent from mission.framework.movement import Depth from mission.framework.targeting import DownwardTarget, HeadingTarget from shm import shape_handle, shape_lightning, shape_banana, shape_soda, shape_bijection CenterCover = Sequential( Depth(1), Concurrent( DownwardTarget((shape_handle.x.get, shape_handle.y.get), target=(200, 200), deadband=(0, 0)))) CenterLightning = Sequential( Depth(1), Concurrent(DownwardTarget((shape_lightning.x.get, shape_lightning.y.get)))) CenterBanana = Sequential( Depth(1), Concurrent(DownwardTarget((shape_banana.x.get, shape_banana.y.get)))) CenterSoda = Sequential( Depth(1), Concurrent(DownwardTarget((shape_soda.x.get, shape_soda.y.get)))) CenterBijection = Sequential( Depth(1), Concurrent(DownwardTarget((shape_bijection.x.get, shape_bijection.y.get))))
XTarget = lambda x, db: PIDLoop(input_value=x, target=0, output_function=VelocityY(), negate=True, p=0.4 if is_mainsub() else 0.4, deadband=db) DEPTH_TARGET = settings.depth #gate = Sequential(target, Log("Targetted"), center, Log("Centered"), charge) # This is the unholy cross between my (Will's) and Zander's styles of mission-writing gate = Sequential( Log('Depthing...'), BigDepth(DEPTH_TARGET), Log('Lining up...'), ConsistentTask(Concurrent( Depth(DEPTH_TARGET), XTarget(x=results_groups.gate_center_x.get, db=0.03), finite=False )), Log('Driving forward...'), MasterConcurrent( Consistent(test=lambda: results_groups.width.get() < settings.gate_width_threshold, count=0.2, total=0.3, invert=True, result=True), Depth(DEPTH_TARGET), VelocityX(0.1 if is_mainsub() else 0.1), While(task_func=lambda: XTarget(x=results_groups.gate_center_x.get, db=0.018), condition=True), ), # Jank Timed(VelocityX(0 if is_mainsub() else -0.1), 2), VelocityX(0), Log('Lining up with red side...'), ConsistentTask(Concurrent( Depth(DEPTH_TARGET),
search_task= lambda: SearchFor(VelocityTSearch(forward=2,stride = 3, rightFirst=PIPE_RIGHT_FIRST), lambda: shm.pipe_results.heuristic_score.get() > 0, consistent_frames=(10, 10)) pitch_search_task = lambda: SearchFor(PitchSearch(30), lambda: shm.pipe_results.heuristic_score.get() > 0, consistent_frames=(6, 6)) pipe_test = lambda: Sequential(Depth(PIPE_SEARCH_DEPTH), search_task(), center(), align(), Depth(PIPE_FOLLOW_DEPTH)) pitch_pipe_test = lambda: Sequential(Depth(PIPE_SEARCH_DEPTH), pitch_search_task(), Zero(), Concurrent(center(), Pitch(0)), Zero(), center(), align(), Depth(PIPE_FOLLOW_DEPTH)) def check_seen(): visible = shm.pipe_results.heuristic_score.get() #print(visible) if visible > 0: return True else: #print('Lost Pipe!') return False class Timeout(Task): def on_first_run(self, time, task, *args, **kwargs): self.timer = Timer(time)
print('leftmost shortest') return shm.gate.leftmost_x align_on_two_elem = lambda:\ Sequential( Log('Can see at least two elements (hopefully)'), Log('Targeting smallest'), ConsistentTask( # while we CAN NOT see all gates # Note: While only consistently succeeds if the inner task finishes and the condition is true FinishIf( task=Concurrent( # pick the element that is smallest focus_elem(get_shortest_elem_visible_x), VelocityX(-0.1), FunctionTask(show), finite=False, ), condition=lambda: gate_elems() == 3 ) ), Log('Found all three elements'), Zero(), finite=False ) align_on_three_elem = lambda:\ Sequential( Log('aligning on three elems'), ConsistentTask(
search_task = lambda: SearchFor( VelocityTSearch(forward=2, stride=3, rightFirst=PIPE_RIGHT_FIRST), lambda: shm.pipe_results.heuristic_score.get() > 0, consistent_frames=(10, 10)) pitch_search_task = lambda: SearchFor(PitchSearch(30), lambda: shm.pipe_results.heuristic_score. get() > 0, consistent_frames=(6, 6)) pipe_test = lambda: Sequential(Depth(PIPE_SEARCH_DEPTH), search_task(), center( ), align(), Depth(PIPE_FOLLOW_DEPTH)) pitch_pipe_test = lambda: Sequential( Depth(PIPE_SEARCH_DEPTH), pitch_search_task(), Zero(), Concurrent(center(), Pitch(0)), Zero(), center(), align(), Depth(PIPE_FOLLOW_DEPTH)) def check_seen(): visible = shm.pipe_results.heuristic_score.get() #print(visible) if visible > 0: return True else: #print('Lost Pipe!') return False class Timeout(Task):
changeleds('p'), PipeQ, Finite(AbsoluteYaw(45)), changeleds('r'), Recovery, EndRun()) Yolo = Sequential(Finite(Depth(2)), QuickTorpedoes, tobins, Bins) FullSemifinals = Sequential(StartRun(), changeleds('g'), Gate, changeleds('p'), tobuoy, changeleds('b'), f, changeleds('p'), towire, changeleds('w'), Wire, changeleds('p'), totorpedoes, QuickTorpedoes, tobins, Bins, changeleds('r'), Recovery, EndRun()) SemifinalsNoPipe = Sequential(StartRun(), start, f, towire, Wire, Track, EndRun()) HydroNV = Sequential(StartRun(), Finite(Depth(.4)), Finite(MoveX(12.7)), Finite(AbsoluteYaw(30)), changeleds('r'), Recovery, EndRun()) Hydro = Sequential(StartRun(), Gate, Finite(MoveX(3)), changeleds('r'), Recovery, EndRun()) testled = changeleds('g') # Transdec = Whatever() Run = Sequential(StartRun(), Concurrent(Unsoftkill(), Semifinals)) Run1 = Recovery Finals = Sequential(StartRun(), changeleds('g'), Finite(Depth(.7)), Finite(MoveX(15.3)), changeleds('b'), RedBuoy, Finite(MoveX(1.8)), changeleds('p'), towire, changeleds('w'), Wire, changeleds('p'), PipeQ, Finite(AbsoluteYaw(45)), changeleds('r'), Recovery, EndRun())
def on_first_run(self, shm_group_getter, is_left, *args, **kwargs): BOTH_DEPTH = settings.pick_up_both_depth SEARCH_DEPTH_1 = settings.pick_up_search_depth_1 SEARCH_DEPTH_2 = settings.pick_up_search_depth_2 SEARCH_DEPTH_3 = settings.pick_up_search_depth_3 START_PICKUP_DEPTH = settings.pick_up_start_follow_depth def downward_target_task(pt=(0, 0), deadband=(0.1, 0.1), max_out=0.04): return DownwardTarget( point=(lambda: shm_group_getter().center_x.get(), lambda: shm_group_getter().center_y.get()), target=norm_to_vision_downward(*pt), deadband=norm_to_vision_downward(-1.0 + deadband[0], -1.0 + deadband[1]), px=0.0005, py=0.001, max_out=max_out, ) 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)), ) bottom_target = (0.8, -0.2) if is_left else (-0.7, -0.4) self.use_task( Sequential( VisionSelector(downward=True), # cons(Depth(BOTH_DEPTH)), # cons(downward_target_task(max_out=0.1)), # stop(), # cons(Depth(SEARCH_DEPTH_1)), # search_at_depth(SEARCH_DEPTH_1, "top", deadband=(0.1, 0.1), depth_timeout=15), # search_at_depth(SEARCH_DEPTH_2, "mid", deadband=(0.05, 0.05), depth_timeout=10), search_at_depth(SEARCH_DEPTH_3, "bot", target=bottom_target, deadband=(0.05, 0.05)), # search_at_depth(3.5, "bot", target=bottom_target, deadband=(0.05, 0.05)), Sequential( *(timed(Depth(depth)) for depth in np.arange(SEARCH_DEPTH_3, START_PICKUP_DEPTH + 0.1, 0.1)) ), timed( Concurrent( Depth(START_PICKUP_DEPTH), Roll(7.5 * (-1 if is_left else 1)), Pitch(-7.5), ) ), Log("PICKING UP??"), timed(Depth(START_PICKUP_DEPTH + 0.3)), Sequential( *(timed(Depth(depth), timeout=1) for depth in np.arange(START_PICKUP_DEPTH, SEARCH_DEPTH_1 + 0.1, 0.1)) ), timed( cons( Concurrent( Depth(SEARCH_DEPTH_1), Roll(0), Pitch(0), ) ), 15 ), ), )
def on_first_run(self, shm_group, is_left, *args, **kwargs): APPROACH_DIST = settings.drop_approach_dist DVL_FORWARD_CORRECT_DIST = settings.drop_dvl_forward_correct_dist[is_left] DVL_ANGLE_CORRECT = settings.drop_heading_correct[is_left] APPROACH_FUNNEL_DEPTH = settings.approach_funnel_depth turn_task = RelativeToInitialHeading(90 if is_left else -90) reset_heading_task = RelativeToInitialHeading(0) reset_heading_task() # reset_pos_task = MoveXYRough((0, 0)) # reset_pos_task() self.reset_pos_target = None drop_task = FireRed if is_left else FireGreen def record_pos(): self.reset_pos_target = (shm.kalman.north.get(), shm.kalman.east.get()) Y_DIST = -APPROACH_DIST if is_left else APPROACH_DIST self.use_task( Sequential( VisionSelector(forward=True), WithPositionalControl( cons(Depth(APPROACH_FUNNEL_DEPTH)) ), ApproachAndTargetFunnel(shm_group), stop(), FunctionTask(record_pos), VisionSelector(downward=True), WithPositionalControl( Sequential( Log("Aligned"), Log("Turning..."), cons(turn_task), Log("Surfacing..."), cons(Depth(-.05)), Log("Moving..."), cons( Concurrent( MoveXYRough((DVL_FORWARD_CORRECT_DIST, Y_DIST)), RelativeToInitialHeading(DVL_ANGLE_CORRECT), finite=False ), debug=True), WithPositionalControl( MasterConcurrent( cons( FunctionTask( lambda: shm.recovery_vision_downward_red.probability.get() > .5, finite=False ), total=30, success=10, debug=True ), VelocityY(.1 * (-1 if is_left else 1)), ), enable=False ), stop(), Log("Over, dropping!..."), drop_task(), # Timer(3), Log("Moving back..."), WithPositionalControl( Sequential( Timed(VelocityY(-.1 * (-1 if is_left else 1)), 2), stop(), ), enable=False ), cons(GoToPositionRough(lambda: self.reset_pos_target[0], lambda: self.reset_pos_target[1]), debug=True), Log("Diving..."), cons(Depth(.5)), Log("Turning back..."), cons(reset_heading_task, debug=True), ) ) ) )
def stop(): return Concurrent( VelocityX(0), VelocityY(0), )