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 approach_right_passageway_task():
align_task = align_right_width()
return FinishIf(
condition=lambda: gate_elems() == 0 or
(align_task.finished and align_task.success),
task=Conditional(
main_task=FunctionTask(lambda: gate_elems() == 1),
on_success=Concurrent(
focus_elem(lambda: shm.gate.leftmost_x, offset=right_offset),
VelocityX(0.2)),
on_fail=Conditional(
main_task=FunctionTask(lambda: gate_elems() >= 2),
on_success=Concurrent(
# Align distance in between the right and middle poles
align_task,
# Align to middle of right and middle poles
PIDLoop(input_value=lambda: (shm.gate.rightmost_x.get() +
shm.gate.middle_x.get()) / 2
if shm.gate.rightmost_visible.get() else
(shm.gate.leftmost_x.get() + shm.gate.middle_x.get(
)) / 2,
target=lambda: shm.gate.img_width.get() / 2,
p=0.3,
deadband=lambda: shm.gate.img_width.get() * 0.05,
output_function=RelativeToCurrentHeading(),
negate=True),
finite=False,
),
on_fail=NoOp(),
finite=False),
finite=False))
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 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 LogSuccess(task):
name = task.__class__.__name__
return Conditional(
task,
Log('{} succeeded!'.format(name)),
Fail(Log('{} failed!'.format(name))),
)
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, 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 TrackerSearch():
return \
Retry(
Sequential(
Conditional(FunctionTask(set_second_task_if_possible), on_fail= \
Sequential(
Depth(BOARD_DEPTH, error=0.2),
# PingerTracker goes here
Conditional(SearchBoard(), on_success=FunctionTask(lambda: set_pinger_task(Stake)), on_fail= \
Sequential(
markers.set('center'),
FunctionTask(lambda: set_pinger_task(Recovery))
)
)
)
)
), attempts=3
)
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, *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, single_bin, *args, **kwargs):
self.use_task(Sequential(
Log('Retracting Amlan'),
AMLANS[0].FastRetract(),
Conditional(
Retry(lambda: ClassifyBins(vision, single_bin), 3),
on_fail=Fail(Log('Failed to ever classify bins')),
),
Conditional(
Retry(lambda: Uncover(vision), 3),
on_fail=Fail(Log('Failed to ever remove cover')),
),
Conditional(
Retry(lambda: Drop(vision), 3),
on_fail=Fail(Log('Failed to ever accurately drop markers')),
),
))
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, vision, *args, **kwargs):
self.use_task(Conditional(
Except(Sequential(
Log('Starting bins'),
Log('Attempting bins assuming both are visible'),
Conditional(
TryBins(vision, single_bin=False),
on_fail=Sequential(
Log('Attempting bins assuming only one is visible'),
TryBins(vision, single_bin=True),
),
)
), Fail(), GlobalTimeoutError),
Log('Bins success! :O'),
Fail(Sequential(Zero(), FastDrop(), Log('Bins failure! :('))),
))
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, 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 TrackerSearch():
global get_pinger_task
global stake
return Retry(lambda: \
Sequential(
Sequential(
Depth(BOARD_DEPTH, error=0.2),
Either(
TrackPinger(),
# Consistent(lambda: get_pinger_task() == stake and shm.torpedoes_stake.board_visible.get(), count=2, total=3, invert=False, result=True)),
Consistent(shm.torpedoes_stake.board_visible.get, count=2, total=3, invert=False, result=True)),
VelocityX(0, error=40),
Log('dafuq'),
Conditional(FunctionTask(set_second_task_if_possible), on_fail= \
Conditional(SearchBoard(), on_success=FunctionTask(lambda: set_pinger_task(stake)), on_fail= \
Sequential(
Log('we cant see jack'),
FunctionTask(lambda: set_pinger_task(surface))
)
)
)
)
), attempts=3
)
def on_first_run(self, vision, reset_state=True, *args, **kwargs):
# Limit navigation speed to prevent merge cutting out computer
nav_speed = shm.navigation_settings.max_speed
orig_speed = nav_speed.get()
nav_speed.set(0.5)
if reset_state:
for loc in ['table', 'tower']:
g_name = getattr(shm, 'recovery_world_{}'.format(loc))
g = g_name.get()
g.know_pos = False
for i in range(self.MAX_COLORS):
setattr(g, 'has_tube{}'.format(i), loc == 'tower'
and i < len(constants.colors))
g_name.set(g)
for a in AMLANS:
a.shm_tube.set(-1)
self.use_task(
Conditional(
Defer(
Sequential(
Log('Starting recovery!'),
AMLANS[0].FastRetract(),
AMLANS[1].FastRetract(),
TryRecovery(vision),
),
Sequential(
AMLANS[0].FastRetract(),
AMLANS[1].FastRetract(),
Log('Returning to pre-recovery depth'),
FastDepth(shm.kalman.depth.get()),
Log('Resetting navigation speed'),
FunctionTask(lambda: nav_speed.set(orig_speed)),
Log('Moving away from table'),
Heading(WALL_TOWER_HEADING),
MoveX(5, deadband=0.08),
),
),
Log('Recovery success!'),
Log('Recovery failure!'),
))
def place_tube(amlan):
return Defer(
Conditional(
Retry(
lambda: Sequential(
Log('Moving above table'),
MoveAboveTable(vision),
Log('Placing {} tube'.format(constants.colors[
amlan.shm_tube.get()].name)),
DropTube(vision, amlan),
), 3),
on_fail=Succeed(
Sequential(
Log('Failed to drop tube normally, performing blind drop'
),
DropTube(vision, amlan, blind=True),
)),
),
amlan.FastRetract(),
)
def DecideAndPush():
counter_wolf = VisibleCounter(shm.bins_status.wolf_visible_frames.get)
counter_bat = VisibleCounter(shm.bins_status.bat_visible_frames.get)
is_bat_getter = lambda: counter_wolf.get_value() < counter_bat.get_value()
counter_wolf_2 = VisibleCounter(shm.bins_status.wolf_visible_frames.get)
counter_bat_2 = VisibleCounter(shm.bins_status.bat_visible_frames.get)
return Sequential(
Depth(0.5),
center_cover(),
DoComparison(counter_wolf, counter_bat),
FunctionTask(lambda: Log('wolf: {}'.format(counter_wolf.get_value()))
()),
FunctionTask(lambda: Log('bat: {}'.format(counter_bat.get_value()))()),
#MasterConcurrent(
Depth(2.5),
# While(center_cover, lambda: True)
#),
LeverWrapper(is_bat_getter=is_bat_getter, wrapped_task=PushLever()),
#Conditional(Timed(FunctionTask(lambda: counter_wolf.get_value() > counter_bat.get_value()), .1),
# on_success=Log("wolf visible"),
# on_fail=Log("bat visible"),
#),
#Log('nani')
Depth(.5),
center_cover(),
DoComparison(counter_wolf_2, counter_bat_2),
FunctionTask(lambda: Log('wolf: {}'.format(counter_wolf_2.get_value()))
()),
FunctionTask(lambda: Log('bat: {}'.format(counter_bat_2.get_value()))
()),
Conditional(
FunctionTask(lambda: decide(is_bat_getter(
), counter_wolf_2.get_value(), counter_bat_2.get_value()),
on_success=full_wolf(),
on_fail=full_bat())))
input_value=lambda: (shm.gate.leftmost_x.get() + shm.gate.rightmost_x.get()) / 2,
target=lambda: shm.gate.img_width.get() / 2,
p=0.2,
deadband=alignment_tolerance_fraction,
output_function=RelativeToCurrentHeading(),
negate=True
),
finite=False
),
condition=lambda: gate_elems() < 3 or is_aligned()
)
),
Conditional(
main_task=FunctionTask(lambda: gate_elems() < 3),
on_success=Sequential(
Log('cannot see all gate_elem'),
Fail()
),
on_fail=Log('is aligned to 3 elems')
),
# finite=False
)
align_on_passageway = lambda:\
ConsistentTask(
PIDLoop(
input_value=lambda: (shm.gate.leftmost_x.get() + shm.gate.middle_x.get()) / 2,
target=lambda: shm.gate.img_width.get() / 2,
p=0.002,
output_function=VelocityY(),
negate=True
),
cls=lambda: Surface(),
modules=[],
surfaces=True,
timeout=timeouts['surface'],
)
track_pinger = lambda: MissionTask(name='Track',
cls=lambda: TrackPinger(depth=1.5),
modules=[],
surfaces=False,
timeout=timeouts['track_pinger'])
SearchTorpedoes = lambda: Retry(lambda: Sequential(
Retry(lambda: Conditional(
SearchFor(TrackPinger(speed=0.25),
shm.torpedoes_stake.board_visible.get,
consistent_frames=(24, 42)),
on_fail=Conditional(SearchBoard(), on_fail=Fail(GoToMarker('gate')))),
attempts=float('inf')), Stake()),
attempts=float('inf'))
TestSearch = lambda: Sequential(
SetMarker('gate'),
SearchTorpedoes(),
)
search_torpedoes = lambda: MissionTask(name='SearchTorpedoes',
cls=SearchTorpedoes,
modules=[shm.vision_modules.Stake],
surfaces=False,
timeout=timeouts['search_torpedoes'])
GrabVampireClosedCoffin = lambda: \
Sequential(
Search(visible_closed),
Center(center_closed, visible_closed),
SetMarker('before_grab'),
Align(center_closed, angle_offset_closed, visible_closed),
Center(offset_closed, visible_closed, db=10),
MasterConcurrent(
Sequential(
Timer(15),
_Grab()),
RelativeToCurrentDepth(DESCEND_DEPTH, error=0.2),
),
RelativeToInitialDepth(-LID_DEPTH_1, error=0.25),
Log('what'),
Conditional(Yike(), on_fail=Fail(_Release())),
GrabVampireOpenCoffin()
)
Yike = lambda: \
Sequential(
MasterConcurrent(
Sequential(Timed(RelativeToCurrentDepth(-LID_DEPTH), 3.5), RelativeToCurrentDepth(0)),
VelocityY(0.2 * direction_closed())
),
Timed(VelocityY(0.3), 3),
Depth(SEARCH_DEPTH, error=0.2),
GoToMarker('before_grab'),
Timeout(Consistent(visible_open, count=1.5, total=2.0, invert=False, result=True), 10),
Log('Opened Coffin Successfully'),
UnsetMarker('before_grab'),
Conditional(
Either(
Sequential(
# Don't lose sight in the first second
Timer(1.0),
# Require a really high fail rate - path vision can be finicky
Consistent(visible_test(1),
count=2.5,
total=3,
result=False,
invert=True),
),
Conditional(
FirstPipeGroupFirst(bend_right),
on_success=FollowPipe(shm.path_results.angle_1, shm.
path_results.angle_2),
on_fail=FollowPipe(shm.path_results.angle_2, shm.
path_results.angle_1)),
),
on_success=Sequential(
Timed(VelocityX(.1), settings.post_dist),
Log("Done!"),
Zero(),
Log("Finished path!"),
),
on_fail=Fail(
Sequential(
Log("Lost sight of path. Backing up..."),
FakeMoveX(-settings.failure_back_up_dist,
speed=settings.failure_back_up_speed),
), ),
),
print('why')
if crucifix_state == 2:
return STATES[0]
return STATES[1]
def crucifix_task():
"""Do crucifix task second. If crucifix not found, do marker 1"""
print('why2')
if crucifix_state == 2:
return STATES[1]
return STATES[0]
Recovery = lambda: Sequential(
Succeed(Timeout(CenterAny(), 20)),
SetMarker('first'),
Conditional(Timeout(SearchCrucifix(), 30), on_success=Succeed(Sequential(CenterCrucifix(), SetMarker('crucifix'), FunctionTask(lambda: change_state(1)))), on_fail=Succeed()),
FunctionTask(reflect),
GoToMarker('second'),
Timeout(SearchAnyVampire(), 40), # TODO: MAKE THIS WORK
Succeed(Timeout(CenterAny(), 40)),
SetMarker('second'),
Conditional(FunctionTask(get_crucifix_found), on_success=Log('Skipping searching crucifix because found already'), on_fail=Conditional(Timeout(SearchCrucifix(), 30), on_success=Sequential(SetMarker('crucifix'), FunctionTask(lambda: change_state(2))), on_fail=Succeed())),
GoToMarker(non_crucifix_task),
# GrabVampire(),
ReleaseVampire(lambda: edge(non_crucifix_task())),
GoToMarker(crucifix_task),
# GrabVampire(),
ReleaseVampire(lambda: edge(crucifix_task())),
Conditional(FunctionTask(get_crucifix_found), on_success=\
Sequential(
GoToMarker('crucifix'),
def withApproachRightHoleOnFail(task):
return lambda *args, **kwargs: Retry(lambda: Conditional(Timeout(task(*args, **kwargs), 120), on_fail=Fail(Sequential(Zero(), Conditional(ReSearchRightHoleOnFail(), on_fail=ApproachRightHole())))), attempts=2)
def withShootRightOnFail(task):
return lambda: Conditional(task(), on_fail=Fail(Sequential(Zero(), ApproachAlign(), ApproachRightHole() if MOVE_DIRECTION == 1 else ApproachLeftHole(), ApproachCloseRight() if MOVE_DIRECTION == 1 else ApproachCloseRight(), FireActuator('bottom_torpedo', 0.3), Backup())))
def on_first_run(self):
self.has_made_progress = True
self.seen_frames_checker = ConsistencyCheck(3, 3, strict=False)
def location_validator(buoy):
return self.seen_frames_checker.check(buoy.probability.get() != 0)
self.depth_task = DepthRestore()
self.heading_task = HeadingRestore()
self.up_task = DepthRestore(constants.BUOY_OVER_DEPTH)
self.dodge_vel = -.4 if yellowRight else .4
self.over_task = Timed(VelocityX(.4), 8)
self.heading_task = HeadingRestore()
self.task = Sequential(
Depth(constants.BUOY_SEARCH_DEPTH),
Conditional(
MasterConcurrent(
Sequential(
self.heading_task,
# Depth(0.9, error=.01)
# SeqLog("Looking for red buoy"), LocateFirstBuoy(lambda: location_validator(red_buoy_results), forward=True, right=BUOY_RIGHT_TO_REACH[0], middle=yellow_buoy_results),
Buoy(red_buoy_results,
first_buoy=True,
right=redRight()),
# self.depth_task,
# SeqLog("Looking for green buoy stage 1"), LocateBuoyStrafe(lambda: location_validator(yellow_buoy_results), right=True, timeout=3),
SeqLog("Looking for green buoy"),
LocateBuoyStrafe(
lambda: location_validator(green_buoy_results),
right=greenRight(),
timeout=3),
Buoy(green_buoy_results, right=greenRight()),
# self.depth_task,
SeqLog("Looking for yellow buoy"),
LocateBuoyStrafe(
lambda: location_validator(yellow_buoy_results),
right=yellowRight(),
timeout=2),
Buoy(yellow_buoy_results,
right=yellowRight(),
yellow=True),
Log("re-aligning red buoy"),
LocateBuoyStrafe(
lambda: location_validator(red_buoy_results),
right=secondRedRight(),
timeout=2),
Buoy(red_buoy_results,
right=secondRedRight(),
yellow=False,
align_only=True),
),
PreventSurfacing(),
),
on_success=Sequential(
Zero(),
self.heading_task,
SeqLog("Rising to Over depth"),
self.up_task,
SeqLog("Going over buoys"),
self.over_task,
),
on_fail=Sequential(
Zero(),
self.heading_task,
SeqLog("Going to Over depth"),
self.up_task,
SeqLog("Going over buoys"),
self.over_task,
Timed(VelocityX(.4), 8),
)))
def withReSearchBoardOnFail(task):
return lambda *args, **kwargs: Retry(lambda: Conditional(Timeout(task(*args, **kwargs), 60), on_fail=Fail(Sequential(Zero(), ReSearchBoardOnFail()))), attempts=2)
wtf = Sequential(Concurrent(PauseTwice(),
Sequential(PauseTwice(), PauseTwice()),
Concurrent(PauseTwice(), PauseTwice())),
PauseTwice())
while not wtf.finished:
wtf()
# Note: A lambda was used to create new instances of the task. Without the lambda, all the tasks would be running the
# same pause_twice task (and finish after only 2 seconds!). With the lambda wtf takes teh expected 12 seconds.
# Other combinators and primitive tasks.
# Conditional runs one task if an argument is true and another if it isn't.
# (NoOp is a task from primitive that simply does nothing)
probably = Conditional(lambda: True, Pause(), NoOp())
## State
# Now, let's use some state.
# SHM should be used as it is normally
class DisplayBuoyInfo(Task):
def run(self):
# Reading from Shared Memory
self.log('Buoy information: X: {0}, Y: {1}, Area: {2}, Probability: {3}'
.format(red_buoy_results.center_x.get(),
red_buoy_results.center_y.get(),
red_buoy_results.area.get(),
red_buoy_results.probability.get()))
def withApproachAlignOnFail(task):
return lambda *args, **kwargs: Retry(lambda: Conditional(Timeout(task(*args, **kwargs), 60), on_fail=Fail(Sequential(Zero(), ApproachAlign(), Zero()))), attempts=2)