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),
