def grab_task(tube, amlan, tries):
return Sequential(
Log('Trying to grab {} tube with {} Amlan'.format(
constants.colors[tube].name,
amlan.name,
)),
Retry(lambda: MoveAboveTower(vision), 3),
Retry(lambda: GetThisTube(vision, tube, amlan), tries),
)
def second_pipe(grp):
return Sequential(
Log('Going to Search Depth'),
Depth(PIPE_SEARCH_DEPTH),
Retry(lambda: Sequential(
Zero(),
Log('Sway searching for pipe'),
search_task(),
Log('Centering on pipe'),
center(),
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 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 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 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, *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, 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, tasks, *args, **kwargs):
#tasks.insert(0, BeginMission)
tasks.append(EndMission)
self.use_task(Retry(lambda: Sequential(
RunTask(BeginMission),
Concurrent(
Sequential(subtasks=[RunTask(t) for t in tasks],),
Fail(WaitForUnkill(killed=False, wait=1)),
),
), float('inf'))
)
def on_first_run(self, vision, *args, **kwargs):
def AvoidWall():
if not shm.recovery_world_table.know_pos.get():
return Sequential(
Log('Moving away from wall'),
ConsistentTask(Heading(WALL_TOWER_HEADING)),
MoveXRough(2),
)
else:
return NoOp()
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),
)
self.use_task(
Except(
Sequential(
Retry(lambda: MoveAboveTable(vision), 1000),
Log('Moving towards wall'),
ConsistentTask(Heading((WALL_TOWER_HEADING + 180) % 360)),
MoveXRough(2),
Iteration(first_run=True),
While(
Iteration,
lambda: count_tubes(shm.recovery_world_tower) > 0,
),
),
Fail(Log('Global timeout, done trying recovery T_T')),
GlobalTimeoutError,
))
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 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 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 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 TestRetry(): return LogSuccess(Retry(lambda: TestSequential(False), 3)) def TestConcurrent(success):
Retry(
task_func=lambda: Sequential(
Log("Searching for path..."),
SearchTask(),
Zero(),
Log("Found Pipe!"),
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),
), ),
),
),
attempts=5))
speed=0.08,
timeout=settings.lost_sight_back_up_timeout
), # we only need to see the first buoy
), ),
),
Zero(),
)
# Perform a task while keeping track of the buoy by updating the consistency checker
TrackBuoy = lambda num, task: MasterConcurrent(
task, FunctionTask(lambda: update_correct_buoy(num)), finite=False)
# Ram either buoy, 0 or 1
RamBuoy = lambda num: Sequential(
# We track the current buoy while ramming and the next one while backing up
TrackBuoy(num, Retry(lambda: RamBuoyAttempt(num), attempts=10)),
TrackBuoy(num + 1, BackUp()),
)
Full = Sequential(
RelativeToInitialHeading(20),
Log('Searching for buoys...'),
SearchBuoy(num=1, count=4,
total=5), # if we see the second buoy then we see both
Succeed(RamBuoy(num=0)),
RelativeToInitialHeading(
10, deadband=5
), # sometimes mission hangs here - due to poorly tuned heading
Succeed(RamBuoy(num=1)),
RelativeToInitialHeading(-10, deadband=5),
)
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.
RamAnything = lambda backspeed=0.2, backtime=10: Sequential(
Log('Failed, backing up'), Zero(), Timeout(SearchTriangle(), 200),
AlignAnyNormal(), ApproachAny(), RamV())
# Decorator that wraps a task to Ram Anything if it fails
withRamAnythingOnFail = lambda task: lambda: Conditional(
main_task=Timeout(task(), 100), on_fail=RamAnything())
# Backs up and search for the triangular buoy again
SearchTriangleOnFail = lambda backspeed=0.2, backtime=10: Sequential(
Log('Failed, backing up'), Zero(), Timed(VelocityX(-backspeed), backtime),
Zero(), Timeout(SearchTriangle(), 120))
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 withApproachAlignOnFail(task):
return lambda *args, **kwargs: Retry(lambda: Conditional(Timeout(task(*args, **kwargs), 60), on_fail=Fail(Sequential(Zero(), ApproachAlign(), Zero()))), attempts=2)
def withReSearchBoardOnFail(task):
return lambda *args, **kwargs: Retry(lambda: Conditional(Timeout(task(*args, **kwargs), 60), on_fail=Fail(Sequential(Zero(), ReSearchBoardOnFail()))), attempts=2)
Full = lambda: Retry(
lambda: Sequential(
Log('Starting'),
Zero(),
#BigDepth(DEPTH_STANDARD, timeout=8),
# Disabled search for now because if tracker mission sees the table, then we don't need to search
Log('Searching for roulette...'),
Search(),
Zero(),
Log('Centering on roulette...'),
align_roulette_center(db=40, p=0.0005),
Log('Descending on roulette...'),
MasterConcurrent(
BigDepth(DEPTH_TARGET_ALIGN_BIN, timeout=8),
align_roulette_center(db=0, p=0.0002),
),
Log('Aligning with table again...'),
align_roulette_center(db=30, p=0.0002),
Log('Aligning heading with green bin...'),
MasterConcurrent(
align_green_angle(db=5, p=0.5),
align_roulette_center(db=0, p=0.0003),
),
Zero(),
Log('Aligning with table again again...'),
MasterConcurrent(
align_roulette_center(db=50, p=0.0003),
align_green_angle(db=0, p=0.5),
),
Zero(),
Log('Descending on table...'),
# Don't align... when we get too close the readings throw things off
PositionalControl(True),
BigDepth(DEPTH_TARGET_DROP, timeout=16),
Zero(),
Log('Dropping ball...'),
DropBall(),
PositionalControl(False),
Log('Returning to normal depth...'),
BigDepth(DEPTH_STANDARD, timeout=8),
Log('Done'),
),
attempts=5)
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'])