def on_run(self, speed): if self.update(): VelocityX(0)() self.finish() return Heading(self.get_target_heading())() VelocityX(self.calc_speed(speed))()
def __init__(self, target_validator, collision_validator, locator_task, concurrent_task=NoOp(), ram_speed=None, *args, **kwargs): """ target_validator - a function that returns True when a target is visible and False otherwise. collision_validator - a function that returns True when a collision is made and False otherwise. concurrent_task - an optional argument for a task to run while moving forward to ram the target. It may be used to continually align with the target while ramming it. ram_speed - a function that returns a speed at which to ram the target """ super().__init__(*args, **kwargs) # self.logv("Starting {} task".format(self.__class__.__name__)) self.target_validator = target_validator self.collision_validator = collision_validator self.ram_speed = ram_speed self.ram_task = VelocityX() self.locator_task = locator_task self.concurrent_task = concurrent_task self.commit_task = Sequential(VelocityX(1), Timer(1), VelocityX(0)) self.ram_commit_phase = False self.TIMEOUT = 25
def on_first_run(self, vision, *args, **kwargs): self.pid = PIDLoop( input_value=lambda: bbar_width_ratio(vision), output_function=VelocityX(), target= constants.BBAR_RATIO_TARGET, deadband=constants.DEFAULT_DEADBAND, p=1, d=4 ) self.retreat_task = Sequential(Log('Retreat'), Timed(VelocityX(-.3), 1))
def __init__(self, timeout, speed=.3, compensate=False, *args, **kwargs): super().__init__(*args, **kwargs) # self.logv("Starting {} task".format(self.__class__.__name__)) self.speed = speed self.ram_task = VelocityX() self.strafe_task = VelocityY() self.commit_task = Sequential(VelocityX(1), Timer(1), VelocityX(0)) self.ram_commit_phase = False self.TIMEOUT = timeout self.compensate = compensate
class Forward(FiniteTask): def firstrun(self): self.go = VelocityX() self.start = time.time() def run(self): self.go(.7) if self.this_run - self.start > 6: self.go(0) self.go._finish() self._finish()
class Forward(Task): def on_first_run(self): self.go = VelocityX() self.start = time.time() def on_run(self): self.go(.7) if self.this_run_time - self.start > 6: self.go(0) self.go.finish() self.finish()
def Spin(): return Sequential( Timed(VelocityX(1), segment_time), Zero(), Heading(initial_heading + 90), Timed(VelocityY(-1), segment_time), Zero(), Heading(initial_heading + 180), Timed(VelocityX(-1), segment_time), Zero(), Heading(initial_heading + 270), Timed(VelocityY(1), segment_time), Zero(), Heading(initial_heading), )
def make_repeat(self, forward, stride, rightFirst, checkBehind): dir = 1 if rightFirst else -1 self.back_check = Timed(VelocityX(-.3), forward) self.checked_b = not checkBehind self.repeat = Sequential( Timed(VelocityX(.3), forward), VelocityX(0.0), Timed(VelocityY(.3 * dir), stride), VelocityY(0.0), Timed(VelocityY(-.3 * dir), stride * 2), VelocityY(0.0), Timed(VelocityY(.3 * dir), stride), VelocityY(0.0), )
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 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 firstrun(self): self.start = time.time() self.surge = VelocityX(.2) self.sway = VelocityY(.2) self.surge() self.sway()
def on_first_run(self, *args, **kwargs): # x-axis on the camera corresponds to sway axis for the sub self.pid_loop_x = PIDLoop(output_function=VelocityY(), negate=True) self.pid_loop_y = PIDLoop(output_function=VelocityX(), negate=False) self.px_default = 0.4 self.py_default = 0.8 PositionalControl(False)()
def on_first_run(self, clockwise=True, steps=5, spins=2, *args, **kwargs): delta_heading = [-1, 1][clockwise] * 45 subspins = [MasterConcurrent(CheckSpinCompletion(2), RelativeToCurrentHeading(delta_heading))] self.use_task(Sequential( Timed(VelocityX(1), 1), Zero(), MasterConcurrent( Sequential(subtasks=subspins), VelocityX(lambda: 2 * math.cos(math.radians(shm.kalman.heading.get()))), VelocityY(lambda: 10 * math.sin(math.radians(shm.kalman.heading.get()))), ), Zero(), ))
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 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 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, 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 run(self): print("Centering on pipe") self.center() if self.center.finished or self.this_run - self.start > 12: VelocityX(0)() VelocityY(0)() self.center._finish() self._finish()
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 Backup(speed=0.2): return Sequential( Timeout(SearchFor( search_task=While(lambda: VelocityX(-speed), True), visible=lambda: visible() and size() < ALIGN_SIZE, consistent_frames=(1.7*60, 2.0*60), ), 15), 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, *args, **kwargs): self.base_heading = shm.kalman.heading.get() self.side = 0 self.loop = 1 self.heading_task = self.make_turn() self.heading_task() self.movement_task = None self.stop_task = VelocityX(0) self.state = "h"
def enter_follow(self): self.logi("Following a heading of %0.3f" % self.heading_to_pinger) self.follow_change_heading = Heading() self.follow_inital_heading = Heading(self.heading_to_pinger + heading_offset) self.follow_vel_x = VelocityX() self.follow_vel_y = VelocityY() distance_to_pinger = self.elevation_to_distance(self.follow_elevation) self.follow_vel_x(x_dir * self.get_follow_speed(distance_to_pinger)) self.follow_vel_y(0.0)
def on_first_run(self, vision, *args, **kwargs): self.use_task(Zeroed(Timeout( Sequential( Log('Attempting to grab handle'), GetHandle(vision, AMLANS[0]), Log('Carrying cover away'), RelativeToInitialDepth(-self.PICKUP_DELTA_DEPTH, error=0.1), Timed(VelocityX(self.SLIDE_SPEED), self.SLIDE_TIME), Zero(), Log('Dropping cover off here'), AMLANS[0].Retract(), Timer(1.5), Log('Attempting to return to near pre-grab location'), RelativeToInitialDepth(-1), Timed(VelocityX(-self.SLIDE_SPEED), self.SLIDE_TIME * 5 / 6), Zero(), ), self.TOTAL_TIMEOUT)))
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 firstrun(self, timeout=45): self.surge_time = time.time() self.sway_time = time.time() - 3 self.start = time.time() self.surge = VelocityX() self.sway = VelocityY() self.sway_speed = .4 self.swaying = True self.surging = False self.begin_search = time.time() self.sway(self.sway_speed) print("IN PIPE")
def on_first_run(self, vision, *args, **kwargs): initial_heading = shm.kalman.heading.get() depth_set = DepthRestore() self.use_task( Conditional( Sequential( MasterConcurrent( Sequential( Retry(lambda: Sequential( Log('Returning to initial heading'), Heading(initial_heading), Log('Going to depth'), depth_set, #Log('Moving forward away last pos'), #Timed(VelocityX(0.5), 1), #Zero(), Log('Searching for gate'), MasterConcurrent( IdentifyGate(vision), VelocityHeadingSearch(initial_heading=initial_heading), ), Zero(), Log('Found gate, aligning'), AlignChannel(vision), ), float('inf')), ), Fail(Timer(180)), ), Log('Aligned to gate, moving closer and fixing depth'), MoveCloser(2), Log('Beginning spin'), StyleSegmentedSpin(), ), Sequential( Log('Wire completed successfully!'), Timed(VelocityX(.4), 2), Zero(), RelativeToInitialHeading(180), ), Log('Traveled too far without task completion'), ) )
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 __init__(self, validator, forward=True, *args, **kwargs): """ validator - a function that returns True when a buoy is found and False otherwise. forward - determines whether the submarine should move forward or backward 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.forward = forward self.surge_task = VelocityX() self.zero_task = Zero() self.TIMEOUT = BUOY_SURGE_TIMEOUT
def on_first_run(self, current_size, current_x, target_size, depth_bounds=(None, None), *args, **kwargs): self.pid_loop_x = PIDLoop(output_function=RelativeToCurrentHeading(), negate=True) self.pid_loop_x_vel = PIDLoop(output_function=VelocityX(), negate=True) self.pid_loop_y = PIDLoop(output_function=RelativeToCurrentDepth( min_target=depth_bounds[0], max_target=depth_bounds[1]), negate=True) self.px_default = 8 self.py_default = 0.8
def on_first_run(self, initial_heading=None, stride_speed=1, stride_time=1.5, heading_amplitude=45, *args, **kwargs): if initial_heading is None: initial_heading = shm.desires.heading.get() Heading(initial_heading)(), self.use_task( While( lambda: Sequential( Timed(VelocityX(stride_speed), stride_time), Zero(), HeadingSearch.Scan(initial_heading + heading_amplitude), HeadingSearch.Scan(initial_heading - heading_amplitude), Heading(initial_heading), ), True))
def on_first_run(self): self.go = VelocityX() self.start = time.time()
def firstrun(self): self.go = VelocityX() self.start = time.time()