def on_first_run(self, is_bat_getter, wrapped_task): is_bat = is_bat_getter() get_x = shm.bins_status.bat_x.get if is_bat else shm.bins_status.wolf_x.get get_y = shm.bins_status.bat_y.get if is_bat else shm.bins_status.wolf_y.get pointing_wrong_way = -1 if is_bat ^ (get_x() < 0) else 1 x_amt = .8 y_amt = 1.5 if is_bat else -1.5 turn_amt = -90 if is_bat else 90 r_x_amt = x_amt * pointing_wrong_way r_y_amt = y_amt * pointing_wrong_way r_turn_amt = turn_amt * pointing_wrong_way ret_y_amt = -.8 if is_bat else .8 self.use_task( Sequential( FakeMoveY(r_y_amt, .3), Timer(.2), FakeMoveX(r_x_amt, .3), Timer(.2), RelativeToInitialHeading(r_turn_amt), wrapped_task, Log("Moving back"), # might have to deal with heaeding here FakeMoveX(-.8, .3), Log("moving Y {}".format(ret_y_amt)), FakeMoveY(ret_y_amt, .3), # intentionally swapped Timer(.5), #Log("moving X {}".format(y_amt * math.sin(math.radians(turn_amt)))), #FakeMoveX(y_amt * math.sin(math.radians(turn_amt)), .3), # intentionally swapped FakeMoveX(1.5, .3), ))
def on_first_run(self, speed=40, db=7, target=180, *args, **kwargs): init_heading = None def set_init_heading(): nonlocal init_heading init_heading = shm.kalman.heading.get() return True set_init_heading() self.use_task( # TODO: DO WE NEED THE WHILE OR DO WE WANT IT TO TERMINATE # While(lambda: \ Sequential( RelativeToInitialHeading(speed), MasterConcurrent( FunctionTask(lambda: (abs(shm.desires.heading.get() - (init_heading + target)) % 360) < db, finite=False), RelativeToCurrentHeading(speed)), # Move back a bit, we might be too close # TODO: DO WE EVEN NEED THIS? # MoveX(-1), # Succeed(FunctionTask(set_init_heading)) ) # , True), )
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, angle, dps=45): self.degrees_per_second = dps self.revolve_angle = 0 self.revolve_task = RelativeToInitialHeading( current=shm.kalman.heading.get()) Pitch(angle)()
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()))
from mission.framework.combinators import Sequential, Concurrent from mission.framework.movement import RelativeToInitialHeading from mission.framework.position import MoveX from mission.framework.primitive import Log sides = 8 tasks = list(map( lambda i: Sequential(Log("Beginning side {}".format(i)), MoveX(4 / sides), RelativeToInitialHeading(360 / sides)), range(sides) )) print(tasks) polygon = Sequential(subtasks=tasks)
def InterMoveY(d): return fake_move_y(d) #return MoveY(d) if os.environ['CUAUV_VEHICLE'] == 'castor' else fake_move_y(d) def startup(): shm.switches.hard_kill.set(0) shm.switches.soft_kill.set(0) def shutdown(): shm.switches.soft_kill.set(1) Qualify = Sequential( RelativeToInitialHeading(0), FunctionTask(startup), Depth(1), Timer(2), Depth(2), Timer(2), Depth(2.5), Timer(2), # Castor InterMoveX(8), Depth(0.8), InterMoveX(7), # # Pollux # InterMoveX(3),
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), ) ) ) )
set_gate = lambda: MissionTask(name="SetGate", cls=lambda: SetMarker('gate'), modules=[], surfaces=False, timeout=20) goto_gate = lambda: MissionTask(name="GoToGate", cls=lambda: GoToMarker('gate'), modules=[], surfaces=False, timeout=60) path = lambda: MissionTask( name="path", cls=lambda: Sequential(Timer(5), RelativeToInitialHeading(-45), Timer(5), MoveX(8, deadband=0.2)), modules=[], surfaces=False, timeout=30) tasks_nonrandom = [ lambda: gate, path, set_gate, track_pinger, lambda: surface, goto_gate, search_torpedoes, ]
CenterCentroid, Disjunction, ConsistentTask, PrintDone, Altitude, AlignAmlan, # AMLANS, Infinite, Except, ) from mission.framework.search import ( SearchFor, VelocitySwaySearch, ) from mission.missions.actuate import ( FireGreen, FireRed, ) from mission.constants.config import cash_in as settings stupid_castor = Sequential( # Timed(VelocityX(0.4), 30), MoveXRough(50), VelocityX(0), RelativeToInitialHeading(50), Timed(VelocityX(0.4), 60), finite=True )
name='GateDead', cls=Sequential( Zero(), Log('Dead reckoning through gate...'), BigDepth(2.2), FakeMoveX(dist=5.8, speed=0.2), ), modules=None, surfaces=False, ) fake_path = lambda bend_right: lambda: MissionTask( name='FakePath', cls=Sequential( FakeMoveX(dist=0.5, speed=0.2), RelativeToInitialHeading(45 if bend_right else -45), ), modules=None, surfaces=False, ) get_path = lambda bend_right: lambda: MissionTask( name='Path', cls=PathGetter(bend_right), modules=[shm.vision_modules.Pipes], surfaces=False, timeout=timeouts['path'], on_timeout=RelativeToInitialHeading(45 if bend_right else -45), ) dice = MissionTask(
math.pi / 2, shm.path_results.angle_1.get() % math.pi, math.pi * 2)) ) % 360)), #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: True)) aaa = Sequential(Log("Started mission"), Depth(1.5), Log("At depth"), FakeMoveX(2, .65), Zero(), Log("Starting tracking"), Timer(1), Log("following"), Timed(Concurrent(VelocityX(.5), FollowLine), 24), Log("following done"), Timed(VelocityX(-.5), .1), Zero(), Timer(1), FakeMoveY(-.6, .65), Timed(VelocityY(.5), .05), Zero(), Timer(1), FakeMoveX(2.8, .65), Timed(VelocityX(-.5), .1), Zero(), Timer(1), Log("turning 1"), RelativeToInitialHeading(90), Log("turning 2"), Timer(1), FakeMoveX(2.8, .65), Timed(VelocityX(-.5), .1), Zero(), Timer(1), Log("turning 1"), RelativeToInitialHeading(90), Log("turning 2"), Timer(1), FakeMoveX(2.8, .65), Timed(VelocityX(-.5), .1), Zero(), Timer(1), FakeMoveY(.8, .65), Timed(VelocityY(-.5), .05), Zero(), Timer(1), Timed(Concurrent(VelocityX(.5), FollowLine), 25), FunctionTask(lambda: shm.switches.soft_kill.set(1))) #bbb = Sequential( # MoveY(.375, deadband=.05), # Timed(Concurrent(VelocityX(.5), FollowLine), 23), #) PipeAlign = lambda heading, trg, dst, deadband: Sequential( Log("PipeAlign start"),
# lambda: shm.vamp_buoy_results.jiangshi_visible.get() and single_buoy_size() > SEARCH_SIZE_THRESH, consistent_frames=(3, 5)), Log('Singular Found'), Zero()) DeadReckonStupid = lambda: \ Sequential( # Timed(VelocityY(DIRECTION * 0.3, error=40), 3), # VelocityY(0, error=40), # Timed(VelocityX(0.3, error=40), 15), # VelocityX(0, error=40), # SlowHeading(), Log('Backing up'), Timed(VelocityX(-0.2), 6), Log('finding buoy'), RelativeToInitialHeading(DIRECTION * 90) ) heading = None def store_heading(h): global heading print('storing heading %s' % h()) heading = h() def get_heading(): global heading if heading == None: heading = shm.kalman.heading.get()
# If this is the second buoy, then don't search again goddamit on_fail=NoOp(), )) # This is the radius of the dots on the die MIN_DOT_RADIUS = settings.min_dot_radius BackUp = lambda: Sequential( Log('Backing up...'), # Should be rammed, back up until we can see both buoys # If we see the second buoy then we can see both BackUpUntilVisible( num=1, speed=0.08, timeout=settings.rammed_back_up_timeout), ) reset_heading = RelativeToInitialHeading(0) RamBuoyAttempt = lambda num: Sequential( Log('Ramming buoy {}'.format(num)), Succeed(reset_heading), Conditional( Either( Consistent(lambda: shm_vars[pick_correct_buoy(num)].visible.get(), count=0.5, total=1.5, invert=True, result=False), Sequential( MasterConcurrent( Consistent(lambda: shm_vars[pick_correct_buoy(num)]. radius_norm.get() < MIN_DOT_RADIUS,