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 ApproachCenterSize(sizef, centerf, alignf, visiblef, size_thresh, p=0.000003, px=0.0009, py=0.005, dx=0.00, dy=0.01, d=0, consistent_total=2.0, closedb=20, db=30000): return MasterConcurrent( Consistent(lambda: abs(sizef()-size_thresh) < db and close_to(centerf(), CAM_CENTER, db=closedb), count=1.7, total=2.0, invert=False, result=True), Consistent(visiblef, count=1.3, total=consistent_total, invert=True, result=False), While(lambda: Center(centerf, visiblef, px=px, py=py, dx=dx, dy=dy), True), PIDStride(lambda: sizef()-size_thresh, p=p, d=d), AlwaysLog(lambda: "center: {}, target: {}, size{}".format(CAM_CENTER, centerf(), sizef())))
def ApproachAlignSize(sizef, centerf, alignf, visiblef, size_thresh, db=30000): return MasterConcurrent( Consistent(lambda: abs(sizef()-size_thresh) < db and aligned(alignf(), db=5), count=1.3, total=2.0, invert=False, result=True), Consistent(visiblef, count=1.3, total=2.0, invert=True, result=False), While(lambda: Align(centerf, alignf, visiblef), True), PIDStride(lambda: sizef()-size_thresh), AlwaysLog(lambda: "center: {}, target: {}, align: {}, size{}".format(CAM_CENTER, centerf(), alignf(), sizef())))
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 on_first_run(self, *args, **kwargs): init_heading = shm.kalman.heading.get() self.use_task(Timed( While(lambda: Sequential( # Pause a little to let object-recognizing tasks see the current fov Timer(0.5), # Check the right side GradualHeading(init_heading + 90), Timer(0.5), Heading(init_heading), # Check the left and back GradualHeading(init_heading - 90), GradualHeading(init_heading - 180), GradualHeading(init_heading - 270), Timer(0.5), Heading(init_heading), # Move back a bit, we might be too close MoveX(-1), ), True), self.TIMEOUT, ))
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 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 on_first_run(self, vision, *args, **kwargs): def AvoidWall(): if not shm.recovery_world_table.know_pos.get(): return Sequential( Log('Moving away from wall'), ConsistentTask(Heading(WALL_TOWER_HEADING)), MoveXRough(2), ) else: return NoOp() def Iteration(first_run=False): return Sequential( # Sequential( # Log('Classifying tubes'), # Retry(lambda: MoveAboveTower(vision), 3), # FunctionTask(vision.classify_tubes), # ) if first_run else NoOp(), GetTube(vision), Succeed(GetTube( vision)), # It's OK if we don't grab the second tube Surface(), AvoidWall(), # Initially classify ellipses as their size / color may change # as tubes are added Sequential( Log('Classifying ellipses'), Retry(lambda: MoveAboveTable(vision), 3), FunctionTask(vision.classify_ellipses), ) if first_run else NoOp(), PlaceTubes(vision), ) self.use_task( Except( Sequential( Retry(lambda: MoveAboveTable(vision), 1000), Log('Moving towards wall'), ConsistentTask(Heading((WALL_TOWER_HEADING + 180) % 360)), MoveXRough(2), Iteration(first_run=True), While( Iteration, lambda: count_tubes(shm.recovery_world_tower) > 0, ), ), Fail(Log('Global timeout, done trying recovery T_T')), GlobalTimeoutError, ))
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 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(), ))
v = heading_to_vector(heading()) * -dst print(shm.path_results.center_x.get(), shm.path_results.center_y.get(), v) return abs(heading_sub_degrees(trg, heading(), math.pi*2)) < math.radians(3) and \ abs(shm.path_results.center_x.get() - v[0]) < deadband and \ abs(shm.path_results.center_y.get() - v[1]) < deadband # def on_run(self, error, p=0.0005, i=0, d=0.0, db=0.01875, max_out=0.5, negate=False, *args, **kwargs): FollowLine = Concurrent( PIDSway(shm.path_results.center_x.get, negate=True, db=0, p=1.2, i=.05), While( lambda: Sequential( FunctionTask(lambda: shm.navigation_desires.heading.set( (shm.kalman.heading.get() - .95 * math.degrees( heading_sub_degrees( 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),
), ) align_task = lambda:\ Sequential( While( condition=lambda: not is_aligned(), task_func=lambda: Sequential( Conditional( main_task=FunctionTask(lambda: gate_elems() == 2), on_success=align_on_two_elem(), on_fail=Conditional( main_task=FunctionTask(lambda: gate_elems() == 3), on_success=align_on_three_elem(), on_fail=Sequential( Log('we see less than two elems, failed'), Timed(VelocityX(-0.3), 2), ), finite=False ), finite=False ), Zero(), #Timer(1), finite=False ), ), finite=False, ) align_left_width = lambda: PIDLoop( input_value=lambda: shm.gate.middle_x.get() - shm.gate.leftmost_x.get(),
def PipeAlign(get_center, heading_vect, heading, get_visible, trg, dst, deadband, angle_range=math.pi * 2, abs_offset=(0, 0)): return Sequential( Log("PipeAlign start"), MasterConcurrent( Consistent(lambda: is_done(heading_vect, heading, get_center, trg, dst, deadband, abs_offset), count=.5, total=.75, invert=False, result=True), While( lambda: Sequential( #Log("attempt asdasd"), Concurrent( DownwardTarget( lambda: get_center(), target=lambda: (np.float64([heading_vect()]).view(np.complex128) * -dst).view(np.float64)[0] + abs_offset, deadband=(deadband, deadband), px=2, py=2, dx=.5, dy=.5 #, ix=.15, iy=.15 ), While( lambda: Sequential( FunctionTask( lambda: shm.navigation_desires.heading.set( (shm.kalman.heading.get( ) - .95 * math.degrees( heading_sub_degrees( trg, heading(), angle_range)) ) % 360) if get_visible() 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()))), Timer(.05)), lambda: abs( heading_sub_degrees(trg, heading(), math.pi * 2 )) > math.radians(5) ) #, count=6, total=8, invert=False, result=True), )), lambda: True #lambda: abs(heading_sub_degrees(trg, heading(), math.pi*2)) > math.radians(5) or abs(shm.path_results.center_x.get()) > .08 or abs(shm.path_results.center_y.get()) > .08 ), #, count=3, total=4, invert=False, result=True), #While(lambda: Log("V: {} h: {} d: {} x: {} y: {} c: {}".format(shm.path_results.num_lines.get(), heading(), math.degrees(heading_sub_degrees(trg, heading(), math.pi*2)), shm.path_results.center_x.get(), shm.path_results.center_y.get(), heading_to_vector(heading()) * dst)), lambda: True), #While(lambda: Log(s(int(math.degrees(heading_sub_degrees(trg, heading(), math.pi*2))) + 180)), lambda: True), #While(lambda: Log(s2(int(math.degrees(trg)), int(math.degrees(heading()))) if shm.path_results.num_lines.get() == 2 else 'X'), lambda: True), ), Log("Centered on Pipe in PipeAlign!"), #FunctionTask(lambda: shm.navigation_desires.heading.set(-180/math.pi*heading()+shm.kalman.heading.get())) )
#While(lambda: Log(s(int(math.degrees(heading_sub_degrees(trg, heading(), math.pi*2))) + 180)), lambda: True), #While(lambda: Log(s2(int(math.degrees(trg)), int(math.degrees(heading()))) if shm.path_results.num_lines.get() == 2 else 'X'), lambda: True), ), Log("Centered on Pipe in PipeAlign!"), #FunctionTask(lambda: shm.navigation_desires.heading.set(-180/math.pi*heading()+shm.kalman.heading.get())) ) def t180(a): return (a + math.pi) % (2 * math.pi) find_bins = lambda: Sequential( Depth(.1), FunctionTask(VelocityX(.5)), While(lambda: NoOp(), lambda: not shm.bins_status.cover_visible.get()), VelocityX(0), ) def get_cover_vect(): return np.float32( [shm.bins_status.cover_maj_x.get(), shm.bins_status.cover_maj_y.get()]) def get_cover_center(): return shm.bins_status.cover_x.get(), shm.bins_status.cover_y.get() def get_wolf_center():
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() )
While( 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 ) #lambda: abs(heading_sub_degrees(trg, heading(), math.pi*2)) > math.radians(5) or abs(shm.path_results.center_x.get()) > .08 or abs(shm.path_results.center_y.get()) > .08 ),
visiblef, consistent_frames=(15, 19) ), Zero(), Depth(INITIAL_DEPTH, error=0.2)) SearchAnyVampire = lambda: Search(any_visible) close_to = lambda point1, point2, dbx=20, dby=20: abs(point1[0]-point2[0]) < dbx and abs(point1[1]-point2[1]) < dby Center = lambda centerf, visiblef, db=15, px=0.001, py=0.001, dx=0.00005, dy=0.00005: Sequential( Log('Centering'), MasterConcurrent( Consistent(lambda: close_to(centerf(), CAM_CENTER, db), count=2.5, total=3.0, invert=False, result=True), Consistent(visiblef, count=2.5, total=3.0, invert=True, result=False), While(lambda: DownwardTarget(point=centerf, target=CAM_CENTER, deadband=(0, 0), px=px, py=py), True), AlwaysLog(lambda: 'center = {}, target = {}'.format(centerf(), CAM_CENTER)))) CenterAny = lambda: Center(center_any, any_visible) # Descend = lambda depth=DEPTH, db=0.1, size_thresh=SIZE_THRESH: Sequential( # TODO: FIND THE ACTUAL DEPTH1!! # Log('Descent into Madness'), # MasterConcurrent( # TODO: TIMEOUT # Consistent(lambda: abs(shm.kalman.depth.get() - depth) < db or size() > size_thresh, count=2.3, total=3, invert=False, result=True), # Depth(depth)), # TODO: BigDepth? # Zero()) close_to = lambda point1, point2, db=20: abs(point1[0]-point2[0]) < db and abs(point1[1]-point2[1]) < db Align = lambda centerf, anglef, visiblef, closedb=10, aligndb=7: Sequential( Log('Aligning'),
visiblef=call_buoy_visible) # Centers the single target buoy CenterSingleBuoy = lambda: CenterBuoy(centerf=single_buoy_center, visiblef=single_buoy_visible) # Approaches a the buoy until it reaches a predetermined size threshold Approach = lambda sizef, centerf, visiblef: Sequential( MasterConcurrent( Consistent(lambda: sizef() > SIZE_THRESH, count=0.2, total=0.3, invert=False, result=True), #ADD EITHER LOSE SIGHT OF BUOY Consistent(visiblef, count=2.5, total=3.0, invert=True, result=False), Succeed(VelocityX(.2)), While(lambda: CenterBuoy(centerf=centerf, visiblef=visiblef), True), AlwaysLog(lambda: "size: {}, visible: {}".format(sizef(), visiblef())) ), Zero()) # Approach only the called buoy @withReSearchCalledOnFail def ApproachCalled(): return Approach(sizef=call_buoy_size, centerf=call_buoy_center, visiblef=call_buoy_visible) # Approach any side of the triangular buoy @withAlignAnyOnFail
# 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), XTarget(x=results_groups.gate_center_x.get, db=0.05), finite=False, )), Log('Charging...'), Timed(VelocityX(0.5 if is_mainsub() else 0.2), settings.charge_dist), Log('Through gate!'), )
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()))
print('flipped: ' + str(h)) if h != last_pinger_heading: last_sub_heading = math.radians(shm.kalman.heading.get()) last_pinger_heading = h return h + last_sub_heading def get_desired_heading(): h = shm_heading() if h < 0: h += math.pi * 2 return math.degrees(h) def get_desired_vel_x(): h = shm_heading() if -math.pi / 4 < h < math.pi / 4: return math.cos(h) * 0.3 # maybe change this function later? else: return 0 track = Sequential( Depth(1.0), While( lambda: Sequential( Heading(get_desired_heading), VelocityX(get_desired_vel_x), ), lambda: True))
Descend = lambda depth=DEPTH, db=0.1, size_thresh=SIZE_THRESH: Sequential( # TODO: FIND THE ACTUAL DEPTH1!! Log('Descent into Madness'), MasterConcurrent( # TODO: TIMEOUT Consistent(lambda: abs(shm.kalman.depth.get() - depth) < db or size() > size_thresh, count=2.3, total=3, invert=False, result=True), Depth(depth)), # TODO: BigDepth? Zero()) close_to = lambda point1, point2, db=10: abs(point1[0] - point2[ 0]) < db and abs(point1[1] - point2[1]) < db Align = lambda closedb=20, aligndb=3: Sequential( Log('Aligning'), MasterConcurrent( Consistent(lambda: close_to(center(), CAM_CENTER) and abs(angle_offset( )) < aligndb, count=2.3, total=3, invert=False, result=True), While(Center, True), PIDHeading(angle_offset) ), Zero()) Grab = lambda: Sequential( MoveY(-0.1), Timeout(RelativeToInitialDepth(0.5), 20), FireActuator(), # TODO )
print(min((45 - diff) / 20, 0.4)) return min((45 - diff) / 20, 0.4) else: print("0") return 0 def enable_hydrophones(): shm.hydrophones_settings.enabled.set(1) track = Sequential( # FunctionTask(enable_hydrophones), Depth(1), Concurrent( While(lambda: FunctionTask(update), lambda: True), While(lambda: Heading(get_target_heading), lambda: True), While(lambda: VelocityX(calc_speed), lambda: True), )) class _TrackPinger(Task): def on_first_run(self, speed=0.4): self.last_shmval = None self.last_target_heading = None self.last_target_elevation = 0 self.checker = ConsistencyCheck(count=3, total=7) def update(self): self.shmval = shm.hydrophones_results_track.tracked_ping_heading.get() # print('last: ' + str(self.last_shmval) + ', new: ' + str(self.shmval))