def on_first_run(self, *args, **kwargs): self.logv("Starting SearchHelperTask task") self.init_time = self.this_run_time self.count = 0 # self.zigzag = Sequential(Finite(MoveY(-SEARCH_SIDE_DISTANCE * 2)), # Finite(MoveX(SEARCH_ADVANCE_DISTANCE)), # Finite(MoveY(SEARCH_SIDE_DISTANCE * 2)), # Finite(MoveX(SEARCH_ADVANCE_DISTANCE))) self.zigzag = Sequential(MoveY(-SEARCH_SIDE_DISTANCE * 2), MoveX(SEARCH_ADVANCE_DISTANCE), MoveY(SEARCH_SIDE_DISTANCE * 2), MoveX(SEARCH_ADVANCE_DISTANCE)) self.zigzag()
class SearchBinsTaskHelper(Task): """Looks around for bins, either covered or uncovered depending on FAST_RUN Suggestion: look around until the probabilities are > 0 for each bin? (see above for values imported from shm) Start: near bins (used pinger to locate), any position Finish: both bins visible in downward cam """ def on_first_run(self, *args, **kwargs): self.logv("Starting SearchHelperTask task") self.init_time = self.this_run_time self.count = 0 # self.zigzag = Sequential(Finite(MoveY(-SEARCH_SIDE_DISTANCE * 2)), # Finite(MoveX(SEARCH_ADVANCE_DISTANCE)), # Finite(MoveY(SEARCH_SIDE_DISTANCE * 2)), # Finite(MoveX(SEARCH_ADVANCE_DISTANCE))) self.zigzag = Sequential(MoveY(-SEARCH_SIDE_DISTANCE * 2), MoveX(SEARCH_ADVANCE_DISTANCE), MoveY(SEARCH_SIDE_DISTANCE * 2), MoveX(SEARCH_ADVANCE_DISTANCE)) self.zigzag() def on_run(self): if self.zigzag.is_finished(): self.count += 1 if self.cycleCount < ((given_distance * PERCENT_GIVEN_DISTANCE_SEARCH * 2) / SEARCH_ADVANCE_DISTANCE) + 1: self.zigzag() else: self.loge("Failed to find bins") self.finish() def on_finish(self): self.logv('SearchHelperTask task finished in {} seconds!'.format( self.this_run_time - self.init_time))
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( Depth(2), Timer(1), Log("following"), Timed(Concurrent(VelocityX(.5), FollowLine), 23), Log("following done"), Timer(1), MoveY(-.375, deadband=.05), MoveX(1.1, deadband=.05),
self._finish() return self.depth() class ToPipe(FiniteTask): def firstrun(self): self.start = time.time() self.count = time.time() def run(self): if shm.pipe_results.heuristic_score.get() < 3000: self.start = time.time() if self.this_run - self.start > .3 or self.this_run - self.count > 60: self._finish() class Pause(FiniteTask): def firstrun(self, wait=0): self.start = time.time() def run(self, wait=0): if self.this_run - self.start > wait: self._finish() Gate = Sequential(dive(), MasterConcurrent(ToPipe(), VelocityX(.4)), VelocityX(0))
def set_APPROACHING(self): self.state = State.APPROACHING self.approach_task = Sequential(DescendTask(APPROACH_DEPTH), ApproachTask())
Sequential( DescendTask(SEARCH_ALIGN_DEPTH), Until(lambda: shm.desires.speed.set(0.1), lambda: contour_count() >= 2 or handle_visible(), lambda: shm.desires.speed.set(0)), Sequential( LazyIf( handle_visible, TimedTask(DownwardTarget(point=handle_point), 10), TimedTask(DownwardTarget(point=most_central_bin_point), 10)), LazyIf( handle_visible, Sequential( LogTask("going for handle"), ThunkTask(lower_grabber), ZeroAfter(WithLazy(handle_heading_abs, OrientTask)), WaitTask(3), ZeroAfter( TimedTask(DownwardTarget(point=handle_point), 10)), DescendTask(HANDLE_GRAB_DEPTH_PRE), LogTask("Descending for pre-grab..."), ZeroAfter( Concurrent( WithLazy(handle_heading_abs, OrientTask), Finite(DownwardTarget(point=handle_point)))), LogTask("Adjusting offset..."), ZeroAfter( TimedTask(RelPosTask(REMOVE_SURGE, REMOVE_SWAY), 8)), DescendTask(HANDLE_GRAB_DEPTH), LogTask("grabbing handle"), ThunkTask(grab_handle), WaitTask(2), DescendTask(DISCARD_DEPTH), DelimitBecome( UntilWith( lambda: shm.kalman.sway.get(), lambda x: shm.desires.sway_speed.set(0.2), lambda x: shm.kalman.sway.get() - x >= 1, lambda x: become( Sequential( ThunkTask(drop_handle), UntilWith( lambda: True, lambda y: shm.desires .sway_speed.set(-0.2), lambda y: abs(shm.kalman.sway.get( ) - x) <= 0.1, lambda y: shm. desires.sway_speed.set(0)))))), ThunkTask(raise_grabber), ZeroAfter( TimedTask(RelPosTask(-REMOVE_SURGE, -REMOVE_SWAY), 8)), ZeroAfter( TimedTask(RelPosTask(DROP_SURGE, DROP_SWAY), 8)), ThunkTask(drop_dropper), WaitTask(2), ZeroAfter( TimedTask(RelPosTask(-DROP_SURGE, -DROP_SWAY), 8)), DescendTask(SEARCH_ALIGN_DEPTH), move_to_next_and_drop(), LogTask("finished!")), Sequential( LogTask("going for two random"), ZeroAfter( TimedTask( DownwardTarget(point=most_central_bin_point), 5)), drop_over_most_central(), WaitTask(2), move_to_next_and_drop(), LogTask("finished!")))))))
def Full(): return Sequential( Log('Changing depth before hydrophones'), Depth(recovery.tower_depth), OptimizablePinger(), )
def TimedLog(msg, t=0.02): return Sequential(Timer(t), Log(msg)) def TestSequential(success):
TrackerGetter = lambda found_roulette, found_cash_in, enable_roulette=True, enable_cash_in=True: Sequential( # Turn on hydromathd ConfigureHydromath(True, enable_cash_in), # Don't kill CPU with vision VisionFramePeriod(track_settings.vision_frame_period), Log('Roulette: ' + str(enable_roulette) + ', Cash-in:' + str(enable_cash_in )), MasterConcurrent( Conditional( # Find either roulette or cash-in Either( Consistent(test=lambda: shm.bins_vision.board_visible.get() if enable_roulette else False, count=1, total=1.5, invert=False, result=True), Consistent(test=lambda: shm.recovery_vision_downward_bin_red. probability.get() > 0 if enable_cash_in else False, count=1, total=1.5, invert=False, result=False), ), # Success is roulette on_success=found_roulette, # Failure is cash-in on_fail=found_cash_in, ), # Track with hydrophones Hydrophones(), ), Zero(), # This should end up getting run twice because we call it in on_exit... but just in case TrackerCleanup(), )
negate=False, p=.000001, i=0, d=0, target=330000, deadband=-2) def run(self): self.pid(input_value=shm.torpedo_results.target_size.get()) # Here, torpedoes x and y would be the center of all the targets-- will actually be calculated once we test TorpedoesTask = Sequential( LineUp(), MissionTarget(), LineUp(error=22), DetermineLid(), HandleLid(), DetermineTargets(), ApproachTarget(index=0), FireTorpedo(actuator=shm.actuator_11.trigger, torpedo=0), MissionTarget(quick=True), ApproachTarget(index=1), FireTorpedo(actuator=shm.actuator_12.trigger, torpedo=1), MissionTarget(quick=True)) QuickTorpedoes = Sequential( MissionTarget(), LineUp(error=22), HandleLid(), ApproachTarget(index=0), FireTorpedo(actuator=shm.actuator_11.trigger, torpedo=0), MissionTarget(quick=True), ApproachTarget(index=1), FireTorpedo(actuator=shm.actuator_12.trigger, torpedo=1), MissionTarget(quick=True)) TorpedoesTaskNoFire = Sequential( LineUp(), MissionTarget(), LineUp(error=22), DetermineLid(), HandleLid(), DetermineTargets(), ApproachTarget(index=0), FireTorpedo(actuator=shm.trax.magx, torpedo=0), MissionTarget(quick=True),