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 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 both_paths():
return Sequential(Log('Going to Search Depth'), Depth(PATH_SEARCH_DEPTH),
Zero(), Log('Beginning to align to Path 1'),
one_path(shm.path_results_1), Log('Moving forward'),
Timed(VelocityX(.2), 1),
Log('Beginning to align to Path 2'),
one_path(shm.path_results_2), Log('Done'), Zero())
def DepthAlign(depth):
return Concurrent(
AlignTargetBin(),
CenterTargetBin(1),
Depth(depth),
finite=False,
)
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 Full(): return Sequential(
Log('Changing depth before hydrophones'),
Depth(settings.depth),
OptimizablePinger(),
)
full = Full()
def on_first_run(self,
north,
east,
heading=None,
depth=None,
optimize=False,
rough=False,
deadband=0.05):
self.north = north
self.east = east
if heading is None:
self.heading = shm.navigation_desires.heading.get()
else:
self.heading = heading
if depth is None:
self.depth = shm.navigation_desires.depth.get()
else:
self.depth = depth
self.use_task(
WithPositionalControl(
Concurrent(PositionN(self.north, error=deadband),
PositionE(self.east, error=deadband),
Heading(self.heading, error=deadband),
Depth(self.depth, error=deadband)),
optimize=optimize,
))
def pitch_pipe(grp):
return Sequential(
Depth(PIPE_SEARCH_DEPTH),
pitch_search_task(),
Zero(),
center(),
Pitch(0),
center(),
Depth(PIPE_FOLLOW_DEPTH),
Concurrent(
#center(),
align(),
finite=False,
),
PositionalControl(),
Zero(),
)
def on_first_run(self, vision, amlan, *args, **kwargs):
self.use_task(Zeroed(Sequential(
Log('Aligning Amlan'),
AlignAmlan(
vision,
vision.target_bin,
amlan,
Depth(constants.above_bin_depth),
),
Log('Moving to handle grab depth'),
Timed(Depth(constants.grab_depth), self.GRAB_TIME),
Log('Extending Amlan'),
amlan.Extend(),
Timer(2),
)))
def on_first_run(self, time, *args, **kwargs):
self.use_task(Sequential(
Depth(constants.WIRE_THRU_DEPTH, error=.1),
Timed(VelocityX(.3), time),
Zero(),
#Timed(VelocityY(.2), 1),
Zero()
))
def on_first_run(self, *args, **kwargs):
self.use_task(
NavigationSpeed(
Sequential(
Depth(recovery_constants.tower_depth, error=0.08),
ConsistentTask(Heading(60 * TOWARDS_BOWL_SIGN)),
MoveX(30, deadband=0.08),
), 0.4))
def on_first_run(self, *args, **kwargs):
self.use_task(
Sequential(
Log('Going to gate depth'),
Depth(constants.depth),
Log('Moving through gate'),
Timed(VelocityX(0.4), constants.vel_time),
Zero(),
))
def on_run(self, pitch=True, roll=True):
Depth(shm.kalman.depth.get())()
PositionN(shm.kalman.north.get(), positional_controls=None)()
PositionE(shm.kalman.east.get(), positional_controls=None)()
Pitch(0)() if pitch else Pitch(shm.kalman.pitch.get())()
Roll(0)() if roll else Roll(shm.kalman.roll.get())()
VelocityX(0, positional_controls=None)()
VelocityY(0, positional_controls=None)()
self.finish()
def on_first_run(self, vision, *args, **kwargs):
self.pid = PIDLoop(
input_value=lambda: bbar_angle(vision),
output_function=VelocityY(),
target=0,
deadband=3,
p=0.8,
d=0.4,
)
self.depth = Depth(constants.WIRE_DEPTH)
def __init__(self, depth=None, *args, **kwargs):
"""
depth - a depth to use as the original depth
"""
super().__init__(*args, **kwargs)
# Store the start depth of the sub
if depth is None:
self.start_depth = constants.BUOY_SEARCH_DEPTH
else:
self.start_depth = depth
self.depth_task = Depth(self.start_depth, error=.01)
def search_at_depth(depth, msg="", target=(0, 0), deadband=(0.1, 0.1), depth_timeout=20):
return Sequential(
timed(cons(Depth(depth)), depth_timeout),
cons(
downward_target_task(target, deadband=deadband),
success=0.80 * 2.5 * 60,
total=2.5 * 60,
debug=True,
),
stop(),
Log("Found at {} (depth={})".format(msg, depth)),
)
def Full():
return Sequential(
Log('Doing balls! RIP octagon'),
Log('Turning away from wall'),
Heading(WALL_TOWER_HEADING, error=5),
Log('Surfacing'),
Depth(0.5, error=0.1),
Timed(Depth(0), 3),
Log('Going over to press octagon'),
Timed(VelocityX(0.3), 11),
Log('Pulling octagon underwater'),
Timed(Concurrent(
VelocityX(0.3),
Depth(0.5),
finite=False,
), 5),
Log('Snapping octagon back in place'),
Timed(VelocityX(-0.2), 4),
VelocityX(0),
Log('Well, I guess that\'s it for balls.'),
)
def on_first_run(self, *args, **kwargs):
original_depth = shm.kalman.depth.get()
self.use_task(
WithPositionalControl(
Sequential(
Log('Getting close to surface'),
FastDepth(0.5),
Log('Surfacing!'),
Timed(Depth(0), 3),
Log('Falling back below surface'),
FastDepth(original_depth),
)))
def on_first_run(self, shm_group, *args, **kwargs):
FUNNEL_DEPTH = settings.approach_funnel_depth
forward_target_task = ForwardTarget(
point=(shm_group.center_x.get, shm_group.center_y.get),
target=norm_to_vision_forward(0.0, 0.4),
depth_bounds=(.45, 1.0),
deadband=norm_to_vision_forward(-0.9, -0.9),
px=0.0004,
py=0.0008,
max_out=.05,
)
heading_target_task = HeadingTarget(
point=(shm_group.center_x.get, shm_group.center_y.get),
target=norm_to_vision_forward(0.0, 0.4),
depth_bounds=(.45, 1.0),
deadband=norm_to_vision_forward(-0.9, -0.9),
px=0.04,
py=0.0008,
max_out=(5, 0.05),
)
self.use_task(
Sequential(
cons(Depth(FUNNEL_DEPTH)),
cons(
heading_target_task,
debug=True
),
cons(
Concurrent(
forward_target_task,
PIDLoop(
input_value=shm_group.area.get,
target=9000,
deadband=250,
output_function=VelocityX(),
reverse=True,
p=0.00008,
max_out=.05,
),
finite=False,
),
total=2.5*60,
success=2.5*60*0.9,
debug=True,
),
)
)
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 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())))
def __init__(self, validator, right=True, timeout=10, *args, **kwargs):
"""
validator - a function that returns True when a buoy is found and False
otherwise.
right - determines whether the submarine should move right or
left during its search
timeout - the amount of time to surge
"""
super().__init__(*args, **kwargs)
# self.logv("Starting {} task".format(self.__class__.__name__))
self.validator = validator
self.right = right
self.surge_task = VelocityY()
self.zero_task = Zero()
self.depth_task = Depth(constants.BUOY_SEARCH_DEPTH)
self.TIMEOUT = timeout
def on_first_run(self, *args, **kwargs):
self.use_task(
WithQuaternionControl(
Sequential(
Log('Time to celebrate!'),
Depth(3, error=0.2),
Pitch(90, error=5),
Log('Weeeeeeeeeee'),
MasterConcurrent(
self.CheckDepth(),
RelativeToCurrentDepth(-0.25),
RelativeToCurrentHeading(10),
),
FunctionTask(
lambda: shm.settings_control.enabled.set(False)),
Zero(),
)))
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 one_path(grp):
return Timeout(
90,
Sequential(
Zero(),
search_task(),
While(
lambda: Sequential(
Zero(),
Log('Centering on path'),
center(grp),
Log('Going to follow depth'),
Depth(PATH_FOLLOW_DEPTH),
Log('Aligning with path'),
Heading(pathAngle(grp.get()), deadband=0.1),
Zero(),
Timer(1),
Log(grp.angle.get()),
), lambda: checkNotAligned(grp.get())),
Log('aligned'),
Zero(),
))
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 = Sequential(Timed(VelocityY(self.dodge_vel), 2),
DirectionalSurge(6, .4, compensate=True))
self.heading_task = HeadingRestore()
self.task = Sequential(
Depth(constants.BUOY_SEARCH_DEPTH),
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=not 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=not greenRight(),
timeout=3),
Buoy(green_buoy_results, right=not greenRight()),
# self.depth_task,
SeqLog("Looking for yellow buoy"),
LocateBuoyStrafe(lambda: location_validator(yellow_buoy_results),
right=not yellowRight(),
timeout=2),
Buoy(yellow_buoy_results, right=not yellowRight(), yellow=True),
HeadingRestore(),
SeqLog("Rising to Over depth"),
self.up_task,
SeqLog("Going around buoys"),
self.over_task)
def on_run(self):
if self.this_run_time - self.first_run_time > self.TIMEOUT:
self.finish()
# self.loge("{} timed out!".format(self.__class__.__name__))
return
# self.logv("Running {}".format(self.__class__.__name__))
# if shm.kalman.depth.get() < .4:
# self.giveup_task()
# self.finish()
if self.validator():
# if abs(shm.kalman.depth.get() - BUOY_SEARCH_DEPTH) >= BUOY_DEPTH_VARIANCE:
# self.depthless_target()
# else:
self.target_task()
else:
# self.heading_task()
# HeadingRestore()
self.logv('lost buoy? searching and restoring depth')
Depth(constants.BUOY_SEARCH_DEPTH)
self.locator_task()
if self.target_checker.check(self.target_task.finished):
self.finish()
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):
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 on_finish(self, *args, **kwargs):
Depth(shm.kalman.depth.get())()
