def on_first_run(self, vision, *args, **kwargs): table = shm.recovery_world_table.get() def record_position(): table.know_pos = True table.north = shm.kalman.north.get() table.east = shm.kalman.east.get() shm.recovery_world_table.set(table) self.use_task( Sequential( # Move to the table depth first so we don't blow things off Log('Moving to table depth'), FastDepth(constants.table_depth), Sequential( Log('Returning to table position ({}, {})'.format( table.north, table.east)), GoToPositionRough(table.north, table.east, optimize=False), ) if table.know_pos else NoOp(), Log('Searching for table'), MasterConcurrent( IdentifyObjects(lambda: vision.ellipses, 2), SearchWithGlobalTimeout(timeout=self.TIMEOUT)), Log('Centering table'), CenterCentroid(lambda: vision.coords(vision.ellipses)), Zero(), Log('Recording table position'), FunctionTask(record_position), ))
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 on_first_run(self, vision, tube, amlan): self.use_task( Sequential( Timeout( AlignAmlan( vision, lambda: vision.obj_with_id(vision.tubes, tube), amlan, Altitude(constants.tube_dive_altitude, overshoot_protect=True, deadband=0.07), align_during_depth=True, ), self.ALIGN_TIMEOUT), Log('Moving to tube grab altitude'), Conditional( Timeout(Altitude(constants.tube_grab_altitude), self.GRAB_ALTITUDE_TIMEOUT), on_fail=Log( 'Warning: grab altitude not fully reached, grabbing anyway' ), ), Timer(1), amlan.Extend(), Timer(1.5), Log('Plucking tube'), Timeout(RelativeToInitialDepth(-0.25, error=0.10), 10), ))
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, 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, vision, amlan, blind=False): initial_tube = amlan.shm_tube.get() def apply_drop(): # Add tube to table table_tubes = get_shm_array(shm.recovery_world_table, 'has_tube') table_tubes[initial_tube] = True set_shm_array(shm.recovery_world_table, 'has_tube', table_tubes) self.use_task( Sequential( AlignAmlan( vision, lambda: vision.obj_with_id(vision.ellipses, amlan.shm_tube.get()), amlan, FastDepth(constants.ellipse_depth), blind=blind, align_during_depth=True, ), Sequential( Log('Moving to tube drop depth'), GradualDepth(constants.drop_depth), ) if not blind else NoOp(), Timer(1), amlan.Retract(), FunctionTask(apply_drop), Sequential( Log('Moving up away from table slowly'), GradualDepth(constants.drop_depth + self.AFTER_DROP_DELTA), ) if not blind else NoOp(), ))
def on_first_run(self, vision, *args, **kwargs): def check_removed(): bin = vision.target_bin() return bin is not None and not bin.obs.covered def CheckRemoved(): return FunctionTask(check_removed) self.use_task(Zeroed(Conditional( Sequential( MoveAboveBins(vision), Conditional( CheckRemoved(), Log('The cover is already gone?!?'), Sequential( Zero(), AlignOverTargetBin(vision, 90), LiftOffCover(vision), Log('Verifying the cover was removed'), MoveAboveBins(vision), CheckRemoved(), ), ), ), on_fail=Fail(Log('Failed to remove cover')), )))
def LogSuccess(task): name = task.__class__.__name__ return Conditional( task, Log('{} succeeded!'.format(name)), Fail(Log('{} failed!'.format(name))), )
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 on_first_run(self, vision, *args, **kwargs): self.use_task(Conditional( Sequential( Retry(lambda: MoveAboveBins(vision), 3), RemoveCover(vision), ), Log('Bin uncovered!'), Fail(Log('Failed to uncover bin')), ))
def on_first_run(self, vision, *args, **kwargs): self.use_task(Conditional( Sequential( Log('Starting drop'), Retry(lambda: MoveAboveBins(vision), 3), AlignOverTargetBin(vision, 90, double_align=True), FireMarkers(), ), on_fail=Fail(Log('Failed to drop')), ))
def on_first_run(self, vision, single_bin, *args, **kwargs): self.use_task(Conditional( Sequential( MoveAboveBins(vision), Timer(0.5), # Wait for vision to stabilize FunctionTask(lambda: vision.classify(single_bin)), ), Log('Bins classified'), Fail(Log('Failed to classify bins')), ))
def on_first_run(self, *args, **kwargs): original_depth = shm.kalman.depth.get() self.use_task( WithPositionalControl( Sequential( Log('Getting close to surface'), FastDepth(0.5), Log('Surfacing!'), Timed(Depth(0), 3), Log('Falling back below surface'), FastDepth(original_depth), )))
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, vision, tube, amlan, *args, **kwargs): def apply_grab(): # Add tube to Amlan in shm amlan.shm_tube.set(tube) # Remove tube from tower in shm tower_tubes = get_shm_array(shm.recovery_world_tower, 'has_tube') tower_tubes[tube] = False set_shm_array(shm.recovery_world_tower, 'has_tube', tower_tubes) # Remove tube from vision pattern vision.remove_tube(tube) north = shm.kalman.north.get() east = shm.kalman.east.get() depth = shm.desires.depth.get() initial_tubes = sum(t.obs is not None for t in vision.tubes) self.use_task( Conditional( Sequential( amlan.FastRetract(), Defer( Conditional( GrabTube(vision, tube, amlan), on_fail=Fail( Sequential( Log('Failed to grab, prematurely retracting grabber' ), amlan.FastRetract(), )), ), Sequential( Zero(), Log('Restoring position from before grab'), FastDepth(depth), GoToPositionRough(north, east, optimize=False), ), ), # TODO continue always assuming good grab? # VerifyGrab(vision, initial_tubes), FunctionTask(apply_grab), ), Log('Grabbed {} tube with {} Amlan!'.format( constants.colors[tube].name, amlan.name, )), Fail(amlan.FastRetract()), ))
def on_first_run(self, vision, *args, **kwargs): self.use_task(Conditional( Sequential( Log('Moving to depth where bins are visible'), Depth(constants.see_both_depth, error=0.2, *args, **kwargs), Log('Searching for bin'), MasterConcurrent(IdentifyBin(vision), SearchWithGlobalTimeout()), Log('Centering bins'), CenterBins(vision), ), on_fail=Fail(Log('Failed to move above bins')), ))
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 on_first_run(self, *args, **kwargs): self.use_task(Sequential( Log('Firing markers!'), MoveX(-self.MARKERS_X_OFFSET, deadband=0.02), FireActuator('left_marker', self.FIRE_TIME), FireActuator('right_marker', self.FIRE_TIME), ))
def Full(): return Sequential( Log('Changing depth before hydrophones'), Depth(settings.depth), OptimizablePinger(), ) full = Full()
def testLeftRestore(): return TrackMovementY( Sequential( Timed(VelocityY(-.4), 6), Log('Restoring?'), RestorePosY(.2), Zero(), ))
def on_first_run(self, *args, **kwargs): self.use_task(Sequential( Log('Ending Run! Surfacing and softkilling'), Zero(), Depth(0), FunctionTask(lambda: shm.switches.soft_kill.set(1)), FunctionTask(lambda: shm.deadman_settings.enabled.set(True)), ))
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 on_first_run(self, vision, amlan, *args, **kwargs): self.use_task(Zeroed(Sequential( Log('Aligning Amlan'), AlignAmlan( vision, vision.target_bin, amlan, Depth(constants.above_bin_depth), ), Log('Moving to handle grab depth'), Timed(Depth(constants.grab_depth), self.GRAB_TIME), Log('Extending Amlan'), amlan.Extend(), Timer(2), )))
def Extend(self): # return SetActuators([self.extend_piston], [self.retract_piston]) return Sequential( Log('Extending {} Amlan'.format(self.name)), SetActuators([], [self.retract_piston]), Timer(0.3), SetActuators([self.extend_piston], []), )
def on_first_run(self, *args, **kwargs): self.use_task( WithQuaternionControl( Sequential( Log('Time to celebrate!'), Depth(3, error=0.2), Pitch(90, error=5), Log('Weeeeeeeeeee'), MasterConcurrent( self.CheckDepth(), RelativeToCurrentDepth(-0.25), RelativeToCurrentHeading(10), ), FunctionTask( lambda: shm.settings_control.enabled.set(False)), Zero(), )))
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 Retract(self): # return SetActuators([self.retract_piston], [self.extend_piston]) return Sequential( # THE THIRD PISTON STATE Log('Retracting {} Amlan'.format(self.name)), SetActuators([self.extend_piston, self.retract_piston], []), Timer(1), SetActuators([self.retract_piston], [self.extend_piston]), self._RemoveTube())
def grab_task(tube, amlan, tries): return Sequential( Log('Trying to grab {} tube with {} Amlan'.format( constants.colors[tube].name, amlan.name, )), Retry(lambda: MoveAboveTower(vision), 3), Retry(lambda: GetThisTube(vision, tube, amlan), tries), )
def on_first_run(self, vision, initial_tubes, *args, **kwargs): if initial_tubes > 1: self.downward_target = Sequential( CenterCentroid(lambda: vision.coords(vision.tubes)), Zero(), ) else: self.downward_target = Succeed( Log('Not targeting tubes to verify grab, none should remain'))
def on_first_run(self, *args, **kwargs): self.killed = shm.switches.hard_kill.get() self.use_task( Sequential( FunctionTask(lambda: shm.switches.soft_kill.set(1)), FunctionTask(lambda: shm.deadman_settings.enabled.set(False)), Log('Disabling Record vision module'), FunctionTask(lambda: shm.vision_modules.Record.set(0)), WaitForUnkill(killed=False, wait=.5), WaitForUnkill(), Log('Zeroing'), Zero(), FunctionTask(lambda: shm.switches.soft_kill.set(0)), EnableController(), Heading(0), Log('Enabling Record vision module'), FunctionTask(lambda: shm.vision_modules.Record.set(1)), ))