def SwayOnlySearch(speed=0.3, width=2.5, right_first=True): direction = 1 if right_first else -1 return Sequential( Log('what'), Timed(VelocityY(direction*speed), width/(2*speed)), Timed(VelocityY(-direction*speed), width/(speed)), Timed(VelocityY(direction*speed), width/(2*speed)), Zero())
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 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 testLeftRestore(): return TrackMovementY( Sequential( Timed(VelocityY(-.4), 6), Log('Restoring?'), RestorePosY(.2), Zero(), ))
def on_first_run(self, depth_bounds=(None, None), *args, **kwargs): self.pid_loop_x = PIDLoop(output_function=VelocityY(), 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 = 0.8 self.py_default = 0.8 PositionalControl(False)()
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 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, 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 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 SearchCalled(): return Sequential( Log('Searching for any buoy'), Zero(), SearchFor( While(lambda: VelocityY(DIRECTION * -0.2), True), triangle_visible, consistent_frames=(3, 5) #TODO: Check consistent frames ), FunctionTask(set_last_seen), Zero())
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
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 __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, 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 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 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)
Sequential( Timer(15), _Grab()), RelativeToCurrentDepth(DESCEND_DEPTH, error=0.2), ), RelativeToInitialDepth(-LID_DEPTH_1, error=0.25), Log('what'), Conditional(Yike(), on_fail=Fail(_Release())), GrabVampireOpenCoffin() ) Yike = lambda: \ Sequential( MasterConcurrent( Sequential(Timed(RelativeToCurrentDepth(-LID_DEPTH), 3.5), RelativeToCurrentDepth(0)), VelocityY(0.2 * direction_closed()) ), Timed(VelocityY(0.3), 3), Depth(SEARCH_DEPTH, error=0.2), GoToMarker('before_grab'), Timeout(Consistent(visible_open, count=1.5, total=2.0, invert=False, result=True), 10), Log('Opened Coffin Successfully'), UnsetMarker('before_grab'), ) grabbing = False def get_grabbing(): global grabbing return grabbing
def on_first_run(self, vel, *args, **kwargs): if shm.jank_pos.y.get() < 0: self.task = VelocityY(vel) else: self.task = VelocityY(vel * -1)
params = [(step, timeout, pause, deadband) for step in depth_steps] #self.use_task(Sequential(*interleave(tasks_from_params(Depth, depth_steps), tasks_from_param(Timer, timeout, length=steps)))) self.use_task( Sequential(*tasks_from_params(Descend, params, tup=True))) FakeMoveX = lambda dist, speed: Sequential( MasterConcurrent(Timer(abs(dist / speed)), VelocityX(-speed if dist < 0 else speed)), MasterConcurrent(Timer(0.2), VelocityX(speed if dist < 0 else -speed)), MasterConcurrent(Timer(0.2), VelocityX(0)), ) FakeMoveY = lambda dist, speed: Sequential( MasterConcurrent(Timer(abs(dist / speed)), VelocityY(-speed if dist < 0 else speed)), MasterConcurrent(Timer(0.2), VelocityY(speed if dist < 0 else -speed)), MasterConcurrent(Timer(0.2), VelocityY(0)), ) # A version of VelocitySwaySearch, but better. # We use this for minisub because it doesn't drift as much. class ForwardSearch(Task): def make_repeat(self, forward, stride, speed, rightFirst): dir = 1 if rightFirst else -1 self.repeat = Sequential( FakeMoveY(stride * speed, speed * dir), FakeMoveX(forward * speed, speed), FakeMoveY(stride * speed, -speed * dir), FakeMoveY(stride * speed, -speed * dir),
import shm results_groups = shm.bicolor_gate_vision class Consistent(Task): def on_first_run(self, test, count, total, invert, result): # Multiple by 60 to specify in seconds self.checker = ConsistencyCheck(count * 60, total * 60, default=False) def on_run(self, test, count, total, invert, result): test_result = call_if_function(test) if self.checker.check(not test_result if invert else test_result): self.finish(success=result) 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
) align_on_three_elem = lambda:\ Sequential( Log('aligning on three elems'), ConsistentTask( # while we CAN see all gates FinishIf( task=Concurrent( # align leftmost and rightmost by length PIDLoop( input_value=lambda: shm.gate.rightmost_len.get() / shm.gate.leftmost_len.get(), target=1, p=0.5, deadband=0.1, output_function=VelocityY() ), # align to the center of the leftmost and rightmost PIDLoop( input_value=lambda: (shm.gate.leftmost_x.get() + shm.gate.rightmost_x.get()) / 2, target=lambda: shm.gate.img_width.get() / 2, p=0.2, deadband=alignment_tolerance_fraction, output_function=RelativeToCurrentHeading(), negate=True ), finite=False ), condition=lambda: gate_elems() < 3 or is_aligned() ) ),
def on_run(self, *args, **kwargs): self.align() if self.align.finished and not self.align.success: VelocityX(0)() VelocityY(0)()
# approach_left = ApproachAndTargetFunnel(shm.recovery_vision_forward_red) # approach_right = ApproachAndTargetFunnel(shm.recovery_vision_forward_red) drop_left = DropInFunnel(shm.recovery_vision_forward_red, is_left=True) drop_right = DropInFunnel(shm.recovery_vision_forward_red, is_left=False) search_left = SearchBin(make_bin_chooser(True)) search_right = SearchBin(make_bin_chooser(False)) pickup_left = PickupFromBin(make_bin_chooser(True), is_left=True) pickup_right = PickupFromBin(make_bin_chooser(False), is_left=False) pickup_all = Sequential( pickup_left, Log("Picked Up Left"), Timed(VelocityY(0.2), 2), VelocityY(0), pickup_right, Log("Picked Up Right"), ) drop_all = Sequential( drop_right, Log("Dropped Right"), drop_left, Log("Dropped Left"), ) do_it_all = Sequential( search_left, Log("Found"),
def stop(): return Concurrent( VelocityX(0), VelocityY(0), )
from mission.framework.combinators import Sequential, Retry, While from mission.framework.movement import RelativeToInitialHeading, VelocityY, VelocityX from mission.framework.position import MoveY from mission.framework.primitive import Log, Fail, Succeed, FunctionTask from mission.framework.timing import Timed from mission.missions.will_common import FakeMoveY sides = 6 def loop_state(): current = -1 def iterate(): nonlocal current current += 1 return current < sides return iterate polygon = Sequential( While( lambda: Sequential(Timed(VelocityY(-0.2), 6), RelativeToInitialHeading(360 / sides), Timed(VelocityY(-0.2), 6)), loop_state()))
def stop(self): VelocityY(0)() RelativeToCurrentDepth(0)()
def make_repeat(self, forward, stride, speed, rightFirst, checkBehind): dir = 1 if rightFirst else -1 if checkBehind: self.repeat = Sequential(Timed(VelocityX(-speed), forward), Timed(VelocityX(speed), forward), VelocityX(0), Timed(VelocityY(speed * dir), stride), VelocityY(0.0), Timed(VelocityX(speed), forward), VelocityX(0.0), Timed(VelocityY(-speed * dir), stride), VelocityY(0.0), Timed(VelocityY(-speed * dir), stride), VelocityY(0.0), Timed(VelocityX(speed), forward), VelocityX(0.0), Timed(VelocityY(speed * dir), stride), VelocityY(0.0)) else: self.repeat = Sequential(Timed(VelocityY(speed * dir), stride), VelocityY(0.0), Timed(VelocityX(speed), forward), VelocityX(0.0), Timed(VelocityY(-speed * dir), stride), VelocityY(0.0), Timed(VelocityY(-speed * dir), stride), VelocityY(0.0), Timed(VelocityX(speed), forward), VelocityX(0.0), Timed(VelocityY(speed * dir), stride), VelocityY(0.0))
def stop(self): VelocityY(0)() VelocityX(0)()
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), ) ) ) )