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 DeadReckonLever():
return Retry(lambda: Sequential(
CenterLever(), # Approach?
_DeadReckonLever(),
Backup(),
Succeed(Timeout(ApproachAlign(), 40)),
Timeout(Consistent(lambda: shm.torpedoes_stake.lever_finished.get(), count=1.5, total=2.0, invert=False, result=True), 10)), attempts=2)
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 on_first_run(self, timeout=60, *args, **kwargs):
self.use_task(Timeout(Sequential(
# Pause initially to give object-identifying tasks time to check current state
Timer(0.5),
SpiralSearch(
relative_depth_range=0,
optimize_heading=True,
meters_per_revolution=1,
min_spin_radius=1,
deadband=0.2,
)
), timeout))
def on_first_run(self, vision, *args, **kwargs):
self.use_task(Zeroed(Timeout(
Sequential(
Log('Attempting to grab handle'),
GetHandle(vision, AMLANS[0]),
Log('Carrying cover away'),
RelativeToInitialDepth(-self.PICKUP_DELTA_DEPTH, error=0.1),
Timed(VelocityX(self.SLIDE_SPEED), self.SLIDE_TIME),
Zero(),
Log('Dropping cover off here'),
AMLANS[0].Retract(),
Timer(1.5),
Log('Attempting to return to near pre-grab location'),
RelativeToInitialDepth(-1),
Timed(VelocityX(-self.SLIDE_SPEED), self.SLIDE_TIME * 5 / 6),
Zero(),
), self.TOTAL_TIMEOUT)))
def on_first_run(self, vision, angle, double_align=False, *args, **kwargs):
def CenterTargetBin(precision):
return CenterBins(vision, lambda bin: bin.id == vision.TARGET_BIN, precision=precision)
def AlignTargetBin():
return AlignHeadingToAngle(lambda: vision.target_bin().obs.angle, angle, mod=180)
def DepthAlign(depth):
return Concurrent(
AlignTargetBin(),
CenterTargetBin(1),
Depth(depth),
finite=False,
)
self.task = Zeroed(Timeout(Sequential(
Log('Centering over target bin'),
CenterTargetBin(0),
Log('Aligning target bin'),
Concurrent(
AlignTargetBin(),
CenterTargetBin(0),
finite=False,
),
Log('Going half down to precisely align with target bin'),
DepthAlign((constants.see_both_depth + constants.above_bin_depth) / 2),
Sequential(
Log('Going down to fully align to bin'),
DepthAlign(constants.above_bin_depth),
) if double_align else NoOp(),
Zero(),
PositionalControl(),
), self.TIMEOUT))
shm.recovery_crucifix.offset_y.get())
def angle_offset_crucifix():
return shm.recovery_crucifix.angle_offset.get()
def size_crucifix():
return shm.recovery_crucifix.size.get()
GrabCrucifix = lambda: \
Sequential(
Search(visible_crucifix),
Center(center_crucifix, visible_crucifix),
Align(center_crucifix, angle_offset_crucifix, visible_crucifix),
Center(offset_crucifix, visible_crucifix, db=10),
MasterConcurrent(
Sequential(
Timer(15),
_Grab()),
RelativeToCurrentDepth(DESCEND_DEPTH, error=0.2),
),
Depth(SEARCH_DEPTH, error=0.2),
Timeout(Consistent(visible_crucifix, count=1.5, total=2.0, invert=True, result=True), 10),
Log('crucifix grabbed successfully'),
)
SearchCrucifix = lambda: Search(visible_crucifix)
CenterCrucifix = lambda: Center(center_crucifix, visible_crucifix)
def _DeadReckonLever():
return Sequential(
Succeed(Timeout(MoveX(.65, deadband=0.05), 20)),
Succeed(Timeout(MoveY(MOVE_DIRECTION * 0.7, deadband=0.05), 20)),
Succeed(Timeout(MoveY(MOVE_DIRECTION * -0.30, deadband=0.1), 20)),
)
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 TestTimeout():
return LogSuccess(Timeout(TimedLog('takes too long to log this', 0.1), 0.05))
BackUpSway = lambda backx=1.5: Sequential(
Log('Backing up'),
MoveX(-backx),
Log('Sway Search in Progress'),
SwaySearch(width=2.5, stride=2)
)
BackUpSwayOnly = lambda backx=0.3: Sequential(
Log('Backing up'),
MoveX(-backx),
)
ReSearchLeftHoleOnFail = lambda timeout=30: Sequential(
Timeout(SearchFor(
BackUpSwayOnly(),
close_left_visible,
consistent_frames=(1.8*60, 2.0*60)
), timeout))
ReSearchRightHoleOnFail = lambda timeout=30: Sequential(
Timeout(SearchFor(
BackUpSwayOnly(),
close_right_visible,
consistent_frames=(1.8*60, 2.0*60)
), timeout))
ReSearchHeartOnFail = lambda timeout=30: Sequential(
Timeout(SearchFor(
BackUpSwayOnly(),
Sequential(
Search(visible_open),
Center(center_open, visible_open, db=20),
SetMarker('before_grab'),
Align(centerf=center_open, anglef=angle_offset_open, visiblef=visible_open),
Center(offset_open, visible_open, db=10),
MasterConcurrent(
Sequential(
Timer(15),
_Grab()),
RelativeToCurrentDepth(DESCEND_DEPTH, error=0.2),
),
Depth(SEARCH_DEPTH),
GoToMarker('before_grab'),
UnsetMarker('before_grab'),
Timeout(Consistent(visible_open, count=1.5, total=2.0, invert=False, result=True), 10),
# Grab(), # ???
# Release???
)
LID_DEPTH = 0.4
LID_DEPTH_1 = 0.5
initial_depth = 3
def record_depth():
global initial_depth
initial_depth = shm.kalman.depth.get()
GrabVampireClosedCoffin = lambda: \
Sequential(
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
)
last_seen = which_buoy_visible()
return True
def get_sway_direction():
global last_seen
return not (getattr(shm.vamp_buoy_results,
"%s_center_x" % last_seen).get() < CAM_CENTER[0])
# Search for buoy using SwayOnlySearch
TinySearch = lambda backspeed=0.2, backtime=3: Sequential(
Zero(),
Log('Doing TinySearch to see if we can find called'),
Timeout(
SearchFor(SwayOnlySearch(right_first=get_sway_direction()),
call_buoy_visible,
consistent_frames=(0.5 * 60, 1.0 * 60)), 20),
FunctionTask(set_last_seen),
Zero(),
)
# Back up, find the triangle buoy again and use it to find the called side
ReSearch = lambda: Sequential(SearchTriangleOnFail(), AlignAnyNormal(),
SearchCalled())
# Decorator that wraps a task to search for the called side of the buoy if it fails
withReSearchCalledOnFail = lambda task: lambda: Retry(lambda: \
Conditional(main_task=task(), on_fail= \
Fail(TinySearch())), attempts=2)
# The final fallback case. If the called side cannot be found, attempt to ram any side of the triangular buoy if possible.
def withReSearchBoardOnFail(task):
return lambda *args, **kwargs: Retry(lambda: Conditional(Timeout(task(*args, **kwargs), 60), on_fail=Fail(Sequential(Zero(), ReSearchBoardOnFail()))), attempts=2)
Log('Approach to gate complete. Beginning alignment'),
align_task(),
FunctionTask(save_heading),
Log('Approaching passageway'),
approach_side_task,
#approach_left_passageway_task,
#approach_right_passageway_task,
Log('Pre Spin Charging...'),
Depth(SPIN_DEPTH_TARGET, error=0.15),
Timed(VelocityX(pre_spin_charge_vel), pre_spin_charge_dist),
Log('Spin Charging...'),
rolly_roll(),
Log('Spin Complete, pausing...'),
Zero(),
Timer(1),
Succeed(Timeout(Heading(lambda: saved_heading, error=5), 5)),
Log('Post Spin Charging...'),
Conditional(
main_task=FunctionTask(lambda: gate_elems() == 2),
on_success=While(
condition=lambda: gate_elems() != 0,
task_func=lambda: VelocityX(0.2),
),
on_fail=Timed(VelocityX(post_spin_charge_vel),
post_spin_charge_dist),
),
Zero(),
Log('Restoring heading'),
Succeed(Timeout(Heading(lambda: saved_heading, error=5), 5)),
Depth(DEPTH_TARGET, error=0.15),
Log('Through gate!')),
def withApproachAlignOnFail(task):
return lambda *args, **kwargs: Retry(lambda: Conditional(Timeout(task(*args, **kwargs), 60), on_fail=Fail(Sequential(Zero(), ApproachAlign(), Zero()))), attempts=2)
def non_crucifix_task():
"""Do non crucifix task first. If crucifix not found, do marker 2"""
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=\
def TestTimeout():
return Timeout(Timer(2), 1)