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 SearchBin(shm_group_getter, count=1.5, total=2):
return Sequential(
VisionSelector(downward=True),
SearchFor(
VelocitySwaySearch(),
lambda: shm_group_getter().probability.get() >= 0.5,
consistent_frames=(count * 60, total * 60) # multiple by 60 to specify in seconds
),
stop(),
)
def SearchCalled():
return Sequential(
Log('Searching for any buoy'),
Zero(),
SearchFor(
While(lambda: VelocityY(DIRECTION * -0.2), True),
triangle_visible,
consistent_frames=(3, 5) #TODO: Check consistent frames
),
FunctionTask(set_last_seen),
Zero())
from mission.framework.jank import TrackMovementY, RestorePosY
from mission.constants.config import path as settings
from mission.constants.region import PATH_1_BEND_RIGHT, PATH_2_BEND_RIGHT
from mission.missions.will_common import Consistent, BigDepth, is_mainsub, FakeMoveX
def visible_test(count):
return lambda: shm.path_results.num_lines.get() >= count
SearchTask = lambda: SearchFor(VelocitySwaySearch(
forward=settings.search_forward,
stride=settings.search_stride,
speed=settings.search_speed,
rightFirst=settings.search_right_first),
visible_test(2),
consistent_frames=(60, 90))
class FirstPipeGroupFirst(Task):
# Checks whether the first pipe group in shm is the first pipe we should follow.
# Succeeds if the first pipe group is consistently the right one, fails otherwise
def on_first_run(self, bend_right):
self.angle_1_checker = ConsistencyCheck(6, 8)
self.angle_2_checker = ConsistencyCheck(6, 8)
def on_run(self, bend_right):
angle_1 = shm.path_results.angle_1.get()
angle_2 = shm.path_results.angle_2.get()
deadband=0,
output_function=VelocityY(),
negate=True)
search_task = lambda:\
SearchFor(
Sequential(
# manual list of "check here first, then just StillHeadingSearch"
FunctionTask(save_heading),
Log('Searching for gate: using manual turning to right'),
Heading(lambda: saved_heading + 90),
Heading(lambda: saved_heading + 180),
Log('Searching for gate: didn\'t find it - turn back'),
Heading(lambda: saved_heading + 90),
Heading(lambda: saved_heading + 0),
Log('Searching for gate: didn\'t find it - spin'),
Heading(lambda: saved_heading + 90),
Heading(lambda: saved_heading + 180),
Heading(lambda: saved_heading + 270),
Heading(lambda: saved_heading + 0),
Log('Searching for gate: fall back on StillHeadingSearch'),
StillHeadingSearch()
),
shm.gate.leftmost_visible.get,
consistent_frames=(6, 10)
)
# main task ###################################################################
gate_full_side = lambda approach_side_task: Sequential(
Log('Depthing...'),
loaded_actuators = {'top_torpedo', 'bottom_torpedo'}
def Fire():
try:
fire = loaded_actuators.pop()
return Sequential(Log('firing %s' % fire), FireActuator(fire, 0.3))
except KeyError:
return Fail(Log('no more torpedoes!'))
SearchBoard = lambda: Sequential(
Log('Searching for torpedo board'),
SearchFor(
StillHeadingSearch(speed=10),
visible,
consistent_frames=(24, 42)
),
Log('Found!'),
Zero()
)
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'),
def on_run(self):
self.update_data()
self.align()
self.center()
if not check_seen():
self.finish(success=False)
if self.alignment_checker.check(self.align.finished):
self.finish()
search_task_behind = lambda: SearchFor(VelocityTSearch(
forward=2, stride=3, rightFirst=PIPE_RIGHT_FIRST, checkBehind=True),
lambda: shm.pipe_results.heuristic_score
.get() > 0,
consistent_frames=(10, 10))
search_task = lambda: SearchFor(
VelocityTSearch(forward=2, stride=3, rightFirst=PIPE_RIGHT_FIRST),
lambda: shm.pipe_results.heuristic_score.get() > 0,
consistent_frames=(10, 10))
pitch_search_task = lambda: SearchFor(PitchSearch(30),
lambda: shm.pipe_results.heuristic_score.
get() > 0,
consistent_frames=(6, 6))
pipe_test = lambda: Sequential(Depth(PIPE_SEARCH_DEPTH), search_task(), center(
), align(), Depth(PIPE_FOLLOW_DEPTH))
return True
return False
def center_any():
if which_visible():
return getattr(shm.recovery_vampire, '%s_handle_x' % which_visible()).get(), getattr(shm.recovery_vampire, '%s_handle_y' % which_visible()).get()
Search = lambda visiblef: Sequential( # TODO: TIMEOUT?
Log('Searching'),
SearchFor(
Sequential(
Depth(SEARCH_DEPTH, error=0.2),
SpiralSearch(),
),
visiblef,
consistent_frames=(15, 19)
),
Zero(),
Depth(INITIAL_DEPTH, error=0.2))
SearchAnyVampire = lambda: Search(any_visible)
close_to = lambda point1, point2, dbx=20, dby=20: abs(point1[0]-point2[0]) < dbx and abs(point1[1]-point2[1]) < dby
Center = lambda centerf, visiblef, db=15, px=0.001, py=0.001, dx=0.00005, dy=0.00005: Sequential(
Log('Centering'),
MasterConcurrent(
Consistent(lambda: close_to(centerf(), CAM_CENTER, db), count=2.5, total=3.0, invert=False, result=True),
Consistent(visiblef, count=2.5, total=3.0, invert=True, result=False),
return shm.recovery_garlic.center_x.get(
), shm.recovery_garlic.center_y.get()
def angle_offset():
return shm.recovery_garlic.angle_offset.get()
def size():
return shm.recovery_garlic.size.get()
Search = lambda: Sequential( # TODO: TIMEOUT?
Log('Searching'),
SearchFor(SwaySearch(width=2.5, stride=2),
visible,
consistent_frames=(5, 7)), Zero())
Center = lambda db=40, px=0.0008, py=0.0008: Sequential(
Log('Centering'),
MasterConcurrent(
DownwardTarget(
point=center, target=CAM_CENTER, deadband=(db, db), px=px, py=py),
AlwaysLog(lambda: 'center = {}, target = {}'.format(
center(), CAM_CENTER))))
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,
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'])
self.finish()
def checkNotAligned(results):
#returns false until aligned
c = abs(results.center_x) < .05 and abs(results.center_y) < .05
a = abs(results.angle) < 95 and abs(results.angle) > 85
print(abs(results.angle))
print(abs(results.center_x))
print(not (c and a))
return not (c and a)
search_task_behind = lambda: SearchFor(VelocityTSearch(
forward=2, stride=3, rightFirst=PATH_RIGHT_FIRST, checkBehind=True),
lambda: shm.path_results_1.visible.get(
) > 0,
consistent_frames=(10, 10))
search_task = lambda: SearchFor(
VelocityTSearch(forward=2, stride=3, rightFirst=PATH_RIGHT_FIRST),
lambda: (shm.path_results_1.visible.get() > 0 and shm.path_results_2.
visible.get() > 0),
consistent_frames=(10, 10))
class Timeout(Task):
def on_first_run(self, time, task, *args, **kwargs):
self.timer = Timer(time)
def on_run(self, time, task, *args, **kwargs):
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.
self.finish()
class mark(Task):
def on_run(self, grp):
cur = grp.get()
cur.north = shm.kalman.north.get()
cur.east = shm.kalman.east.get()
cur.depth = shm.kalman.depth.get()
cur.heading = math.radians(shm.pipe_results.angle.get())
grp.set(cur)
self.finish()
search_task = lambda: SearchFor(SwaySearch(1, 1),
lambda: shm.pipe_results.heuristic_score.get()
> 0,
consistent_frames=(6, 6))
pipe_test = lambda: Sequential(Depth(PIPE_SEARCH_DEPTH), search_task(), center(
), align(), Depth(PIPE_FOLLOW_DEPTH))
one_pipe = lambda grp: Sequential(Depth(PIPE_SEARCH_DEPTH), search_task(
), center(), align(), mark(grp), Depth(PIPE_FOLLOW_DEPTH))
pipe_mission = Sequential(one_pipe(shm.desires), one_pipe(shm.desires))
class Timeout(Task):
def on_first_run(self, time, task, *args, **kwargs):
self.timer = Timer(time)
def on_run(self, time, task, *args, **kwargs):
task()
shm_vars[pick_correct_buoy(num)].center_y.get()),
target=(0, 0.1),
deadband=(db, db),
px=0.08 * mult,
py=0.08 * mult, # y is depth
depth_bounds=(1, MAX_DEPTH))
# num here refers to the shm group, not the tracked num
SearchBuoy = lambda num, count, total: Sequential(
BigDepth(BUOY_DEPTH),
Either(
SearchFor(
ForwardSearch(forward=settings.search_forward,
stride=settings.search_stride,
speed=settings.search_speed),
shm_vars[num].visible.get,
consistent_frames=(count * 60, total * 60
) # multiple by 60 to specify in seconds
),
# Time out if we can only see one die
Sequential(
Timer(settings.search_default_zero_timeout),
Consistent(lambda: shm_vars[0].visible.get(),
count=count,
total=total,
result=True,
invert=False),
),
),
)
HEADING_OFFSET = settings.heading_offset
align_roulette_center = lambda db=20, p=0.0005: DownwardTarget(
(BIN_CENTER[0].get, BIN_CENTER[1].get),
target=CAM_CENTER,
px=p,
py=p,
deadband=(db, db))
align_green_angle = lambda db=10, p=0.8: DownwardAlign(
lambda: GREEN_ANGLE.get() + HEADING_OFFSET, target=0, deadband=db, p=p)
DropBall = lambda: FireBlue()
Search = lambda: SearchFor(
SaneHeadingSearch(),
shm.bins_vision.board_visible.get,
consistent_frames=(1 * 60, 1.5 * 60
) # multiply by 60 to specify in seconds
)
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...'),