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 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 TestConcurrent(success):
return LogSuccess(Concurrent(
Succeed(TimedLog('1', 0.03), success),
TimedLog('2', 0.04),
TimedLog('3', 0.01),
TimedLog('4', 0.02),
))
def TestSequential(success):
return LogSuccess(Sequential(
TimedLog('1'),
TimedLog('2'),
TimedLog('3'),
Succeed(TimedLog('4'), success),
TimedLog('5'),
))
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 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 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),
)
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 on_first_run(self, main_task, deferred, *args, **kwargs):
self.task = Sequential(Succeed(main_task), deferred)
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 _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)),
)
# ApproachLeftHole() if MOVE_DIRECTION == 1 else ApproachRightHole(),
# ApproachCloseLeft(),
# FireActuator('bottom_torpedo', 0.5),
# Backup(),
# Log('Stake complete')
# )
Full = \
lambda: Sequential(
Log('Starting Stake'),
# Depth(BOARD_DEPTH, error=0.2),
# Timeout(SearchBoard(), 60),
FireActuator('top_torpedo', 0.3),
Succeed(Timeout(ApproachAlign(), 20)),
Log('what'),
# ApproachLeftHole() if MOVE_DIRECTION == -1 else ApproachRightHole(),
# ApproachCloseLeft() if MOVE_DIRECTION == -1 else ApproachCloseRight(),
# Backup(),
DeadReckonLever(),
ApproachLeftHole() if MOVE_DIRECTION == 1 else ApproachRightHole(),
ApproachCloseLeft() if MOVE_DIRECTION == 1 else ApproachCloseRight(),
FireActuator('bottom_torpedo', 0.3),
Backup(),
Log('Stake complete')
)
Test = \
lambda: Sequential(
def set_grabbing(value):
global grabbing
grabbing = value
return Succeed()
CenterCalledBuoy = lambda: CenterBuoy(centerf=call_buoy_center,
visiblef=call_buoy_visible)
# Centers the single target buoy
CenterSingleBuoy = lambda: CenterBuoy(centerf=single_buoy_center,
visiblef=single_buoy_visible)
# Approaches a the buoy until it reaches a predetermined size threshold
Approach = lambda sizef, centerf, visiblef: Sequential(
MasterConcurrent(
Consistent(lambda: sizef() > SIZE_THRESH,
count=0.2,
total=0.3,
invert=False,
result=True), #ADD EITHER LOSE SIGHT OF BUOY
Consistent(visiblef, count=2.5, total=3.0, invert=True, result=False),
Succeed(VelocityX(.2)),
While(lambda: CenterBuoy(centerf=centerf, visiblef=visiblef), True),
AlwaysLog(lambda: "size: {}, visible: {}".format(sizef(), visiblef()))
),
Zero())
# Approach only the called buoy
@withReSearchCalledOnFail
def ApproachCalled():
return Approach(sizef=call_buoy_size,
centerf=call_buoy_center,
visiblef=call_buoy_visible)
# Approach any side of the triangular buoy
MasterConcurrent(
Consistent(lambda: shm_vars[num].visible.get(),
count=1,
total=1.5,
result=True,
invert=False),
VelocityX(-speed),
),
),
Fail(
Timer(timeout), # don't back up too far
),
),
deferred=Sequential(VelocityX(0), ),
),
on_success=Succeed(Sequential(NoOp(), ), ),
on_fail=Conditional(
FunctionTask(lambda: num == 0),
on_success=Sequential(
Log('Timed out, searching for buoy again'),
SearchBuoy(num=num, count=4, total=5),
),
# If this is the second buoy, then don't search again goddamit
on_fail=NoOp(),
))
# This is the radius of the dots on the die
MIN_DOT_RADIUS = settings.min_dot_radius
BackUp = lambda: Sequential(
Log('Backing up...'),