def approach_right_passageway_task():
align_task = align_right_width()
return FinishIf(
condition=lambda: gate_elems() == 0 or
(align_task.finished and align_task.success),
task=Conditional(
main_task=FunctionTask(lambda: gate_elems() == 1),
on_success=Concurrent(
focus_elem(lambda: shm.gate.leftmost_x, offset=right_offset),
VelocityX(0.2)),
on_fail=Conditional(
main_task=FunctionTask(lambda: gate_elems() >= 2),
on_success=Concurrent(
# Align distance in between the right and middle poles
align_task,
# Align to middle of right and middle poles
PIDLoop(input_value=lambda: (shm.gate.rightmost_x.get() +
shm.gate.middle_x.get()) / 2
if shm.gate.rightmost_visible.get() else
(shm.gate.leftmost_x.get() + shm.gate.middle_x.get(
)) / 2,
target=lambda: shm.gate.img_width.get() / 2,
p=0.3,
deadband=lambda: shm.gate.img_width.get() * 0.05,
output_function=RelativeToCurrentHeading(),
negate=True),
finite=False,
),
on_fail=NoOp(),
finite=False),
finite=False))
def on_first_run(self,
vision,
obj_func,
amlan,
align_depth_task,
blind=False,
align_during_depth=False,
*args,
**kwargs):
def tube_angle_heading():
return shm.kalman.heading.get() + (obj_func().obs.angle % 180 - 90)
self.must_see_obj = not blind
def unsee():
self.must_see_obj = False
self.task = Sequential(
Sequential(
Log('Centering object'),
CenterCentroid(lambda: vision.coords([obj_func()]),
precision=0),
Zero(),
Sequential(
Log('Aligning to object'),
Concurrent(
CenterCentroid(lambda: vision.coords([obj_func()]),
precision=0),
GradualHeading(tube_angle_heading),
finite=False,
),
) if not align_during_depth else NoOp(),
Log('Going down and precisely aligning to object'),
ConsistentTask(
Concurrent(
GradualHeading(tube_angle_heading),
CenterCentroid(lambda: vision.coords([obj_func()]),
precision=2),
align_depth_task,
finite=False,
)),
FunctionTask(unsee),
) if not blind else NoOp(),
PositionalControl(),
Zero(),
Log('Applying Amlan offset'),
amlan.CenterFromDownCam(),
)
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):
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 DepthAlign(depth):
return Concurrent(
AlignTargetBin(),
CenterTargetBin(1),
Depth(depth),
finite=False,
)
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 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 set_REMOVING(self, xp, yp, hp):
self.state = State.REMOVING
self.remove_start = time.time()
self.old_x = xp
self.old_y = yp
self.old_h = hp
self.removing_initial_hd = normalized_hd()
# handle losing sight of the handle better
# XXX will need to be tweaked, Zander is tweaking vision GUI, grabber, etc
self.remove_positioning_task = Sequential(
OrientTask(lambda: self.removing_initial_hd),
ZeroTask(),
WaitTask(3),
Finite(DownwardTarget(point=self.handlef, min_out=0.1)),
ZeroTask(),
DescendTask(HANDLE_GRAB_DEPTH_PRE),
LogTask("Descending for pre-grab..."),
Concurrent(OrientTask(lambda: self.removing_initial_hd),
Finite(DownwardTarget(point=self.handlef))),
ZeroTask(),
LogTask("Adjusting offset..."),
TimedTask(RelPosTask(REMOVE_SURGE, REMOVE_SWAY), 8),
# note that they are flipped here because the sub is rotated 90 when dropping/removing
ZeroTask(),
#DistSurgeTask(d=REMOVE_SURGE),
#DistSwayTask(d=REMOVE_SWAY),
DescendTask(HANDLE_GRAB_DEPTH),
LogTask("grabbing handle"),
ThunkTask(grab_handle),
WaitTask(2),
DescendTask(DISCARD_DEPTH))
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 on_first_run(self, vector, deadband=0.1):
vector = call_if_function(vector)
self.use_task(
WithPositionalControl(
Concurrent(PositionN(target=vector[0], error=deadband),
PositionE(target=vector[1], error=deadband),
finite=False))
)
def on_first_run(self, vector, deadband=0.01, *args, **kwargs):
delta_north, delta_east = rotate(vector, kalman.heading.get())
n_position = RelativeToInitialPositionN(offset=delta_north,
error=deadband)
e_position = RelativeToInitialPositionE(offset=delta_east,
error=deadband)
self.use_task(
WithPositionalControl(
Concurrent(n_position, e_position, finite=False), ))
def DoComparison(counter_a, counter_b):
return \
While(
lambda: Timed(Concurrent(
While(center_cover, lambda: True),
counter_a,
counter_b,
), 1.5),
lambda: counter_a.get_value() + counter_b.get_value() < 10
)
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, 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 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 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, 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))
printer_two = lambda one, two: RunTwo(Printer(message=one), Printer(message=two))
# And then use it.
printer_remix = printer_two('Hello', 'but actually...')
printer_remix()
# However, although RunTwo will work properly, it's not very good style.
# We can easily create a task that will run any number of subtasks in parallel.
# (which is a nice segue into...)
## Combinators
# Concurrent runs multiple tasks at the same time. Here, again, we're binding some default arguments to the run method.
# Note: This will initialize 2 separate Printer tasks.
printer_twice = Concurrent(Printer("Concurrent 1"), Printer("Concurrent 2"))
printer_twice()
# We can also pass a list of tasks, with the "subtasks" keyword parameter.
# Note: This will initialize 3 separate Printer tasks.
printer_three_times = Concurrent(subtasks=[Printer(i) for i in range(3)])
printer_three_times()
# Tasks can be generated in procedural ways.
# Note: This will initialize 3 separate Printer tasks.
printer_indeed = Concurrent(subtasks=[Printer(message=s) for s in ['Pie', 'Cake', 'American Politics']])
printer_indeed()
# Sequential runs multiple tasks in sequence, but it requires tasks to implement certain methods.
# Let's create a rather silly task to demonstrate.
def PushLever():
def TargetLever(py):
return ForwardTarget(
point=lambda:
(shm.bins_status.lever_x.get(), shm.bins_status.lever_y.get() /
(1 + shm.bins_status.lever_sz.get() / 50)),
#target=lambda: (0, .3 * min(100, shm.bins_status.lever_sz.get()) / 100),
target=lambda: (0, .25 /
(1 + shm.bins_status.lever_sz.get() / 50)),
valid=shm.bins_status.lever_visible.get,
deadband=(DEADBAND, DEADBAND),
px=1,
py=py,
ix=.05,
iy=.05)
return Sequential(
Log("PushLever start"),
FunctionTask(VelocityX(.2)),
MasterConcurrent(
Consistent(lambda: shm.bins_status.lever_sz.get() > 90,
count=.5,
total=.75,
invert=False,
result=True),
While(lambda: TargetLever(1.5), lambda: True),
While(
lambda: FunctionTask(
VelocityX(.2 / (1 + 2 * (abs(shm.bins_status.lever_x.get(
)) + abs(shm.bins_status.lever_y.get() - .25))))),
lambda: True)),
#Log("Higher P"),
#MasterConcurrent(
# Consistent(lambda: shm.bins_status.lever_sz.get() > 100, count=.5, total=.75, invert=False, result=True),
# While(lambda: TargetLever(.8), lambda: True),
# While(lambda: FunctionTask(
# VelocityX(.2 / (1 + 2 * (abs(shm.bins_status.lever_x.get()) + abs(shm.bins_status.lever_y.get()-.15))))
# ), lambda: True)
#),
#Log("targeting"),
#TargetLever(),
Log("zoom zoom"),
#RelativeToInitialDepth(-.1),
#Timed(RelativeToCurrentDepth(-2), .7),
#FunctionTask(RelativeToInitialDepth(0)),
Concurrent(
Sequential(
Timer(1),
Log("Forwards!"),
FunctionTask(VelocityX(1)),
#VelocityDepth(0),
),
Sequential(
Timed(RelativeToCurrentDepth(-.4), 1),
RelativeToInitialDepth(0),
),
Timer(4),
),
#Timed(
# While(TargetLever, lambda: True),
# 5
#),
Timed(VelocityX(-.8), .5),
VelocityX(0),
Log("waiting"),
Timer(4),
#RelativeToInitialHeading(0),
#Timed(VelocityX(-.8), 1),
#RelativeToInitialHeading(0),
#FunctionTask(VelocityX(0)),
#Timer(5),
#TargetLever()
)
lambda: Sequential(
Log("attempt asdasd"),
Concurrent(
DownwardTarget(lambda: (shm.path_results.center_x.get(),
shm.path_results.center_y.get()),
target=(lambda: heading_to_vector(heading())
* -dst),
deadband=(deadband, deadband),
px=1,
py=1,
ix=.05,
iy=.05),
While(
lambda: Sequential(
FunctionTask(lambda: shm.navigation_desires
.heading.set((shm.kalman.heading.get(
) - .95 * math.degrees(
heading_sub_degrees(
trg, heading(), math.pi * 2)))
% 360)
if shm.path_results.num_lines.get(
) == 2 else None),
#Log('{:03d} '.format(int(shm.navigation_desires.heading.get())) + s2(int(math.degrees(trg)), int(math.degrees(heading())), int(shm.desires.heading.get())) if shm.path_results.num_lines.get() == 2 else 'X'),
Timer(.05)),
lambda: abs(
heading_sub_degrees(trg, heading(), math.pi * 2)
) > math.radians(5)
) #, count=6, total=8, invert=False, result=True),
)),
lambda: not is_done(
heading, trg, dst, deadband
# TODO this might not be working
print(angle_1, angle_2, diff)
if self.angle_1_checker.check(
diff > 0 ^ (angle_1 < angle_2) ^ (not bend_right)):
self.finish()
if self.angle_2_checker.check(
diff < 0 ^ (angle_1 < angle_2) ^ (not bend_right)):
self.finish(success=False)
PipeAlign = lambda heading: Concurrent(
DownwardTarget(lambda: (shm.path_results.center_x.get(),
shm.path_results.center_y.get()),
target=(0, -.25),
deadband=(.1, .1),
px=0.5,
py=0.5), Log("Centered on Pipe!"),
FunctionTask(lambda: shm.navigation_desires.heading.set(
-180 / 3.14 * heading.get() + shm.kalman.heading.get())))
FollowPipe = lambda h1, h2: Sequential(
PipeAlign(h1),
Zero(),
Log("Aligned To Pipe!"),
DownwardTarget(lambda: (shm.path_results.center_x.get(),
shm.path_results.center_y.get()),
target=(0, 0),
deadband=(.1, .1),
px=0.5,
py=0.5),
#!/usr/bin/env python3.4
from mission.framework.combinators import Sequential, Concurrent
from mission.framework.movement import Depth
from mission.framework.targeting import DownwardTarget, HeadingTarget
from shm import shape_handle, shape_lightning, shape_banana, shape_soda, shape_bijection
CenterCover = Sequential(
Depth(1),
Concurrent(
DownwardTarget((shape_handle.x.get, shape_handle.y.get),
target=(200, 200),
deadband=(0, 0))))
CenterLightning = Sequential(
Depth(1),
Concurrent(DownwardTarget((shape_lightning.x.get, shape_lightning.y.get))))
CenterBanana = Sequential(
Depth(1),
Concurrent(DownwardTarget((shape_banana.x.get, shape_banana.y.get))))
CenterSoda = Sequential(
Depth(1), Concurrent(DownwardTarget((shape_soda.x.get, shape_soda.y.get))))
CenterBijection = Sequential(
Depth(1),
Concurrent(DownwardTarget((shape_bijection.x.get, shape_bijection.y.get))))
XTarget = lambda x, db: PIDLoop(input_value=x, target=0,
output_function=VelocityY(), negate=True,
p=0.4 if is_mainsub() else 0.4, deadband=db)
DEPTH_TARGET = settings.depth
#gate = Sequential(target, Log("Targetted"), center, Log("Centered"), charge)
# This is the unholy cross between my (Will's) and Zander's styles of mission-writing
gate = Sequential(
Log('Depthing...'),
BigDepth(DEPTH_TARGET),
Log('Lining up...'),
ConsistentTask(Concurrent(
Depth(DEPTH_TARGET),
XTarget(x=results_groups.gate_center_x.get, db=0.03),
finite=False
)),
Log('Driving forward...'),
MasterConcurrent(
Consistent(test=lambda: results_groups.width.get() < settings.gate_width_threshold, count=0.2, total=0.3, invert=True, result=True),
Depth(DEPTH_TARGET),
VelocityX(0.1 if is_mainsub() else 0.1),
While(task_func=lambda: XTarget(x=results_groups.gate_center_x.get, db=0.018), condition=True),
),
# Jank
Timed(VelocityX(0 if is_mainsub() else -0.1), 2),
VelocityX(0),
Log('Lining up with red side...'),
ConsistentTask(Concurrent(
Depth(DEPTH_TARGET),
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))
pitch_pipe_test = lambda: Sequential(Depth(PIPE_SEARCH_DEPTH),
pitch_search_task(), Zero(),
Concurrent(center(), Pitch(0)), Zero(),
center(), align(), Depth(PIPE_FOLLOW_DEPTH))
def check_seen():
visible = shm.pipe_results.heuristic_score.get()
#print(visible)
if visible > 0:
return True
else:
#print('Lost Pipe!')
return False
class Timeout(Task):
def on_first_run(self, time, task, *args, **kwargs):
self.timer = Timer(time)
print('leftmost shortest')
return shm.gate.leftmost_x
align_on_two_elem = lambda:\
Sequential(
Log('Can see at least two elements (hopefully)'),
Log('Targeting smallest'),
ConsistentTask(
# while we CAN NOT see all gates
# Note: While only consistently succeeds if the inner task finishes and the condition is true
FinishIf(
task=Concurrent(
# pick the element that is smallest
focus_elem(get_shortest_elem_visible_x),
VelocityX(-0.1),
FunctionTask(show),
finite=False,
),
condition=lambda: gate_elems() == 3
)
),
Log('Found all three elements'),
Zero(),
finite=False
)
align_on_three_elem = lambda:\
Sequential(
Log('aligning on three elems'),
ConsistentTask(
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))
pitch_pipe_test = lambda: Sequential(
Depth(PIPE_SEARCH_DEPTH), pitch_search_task(), Zero(),
Concurrent(center(), Pitch(0)), Zero(), center(), align(),
Depth(PIPE_FOLLOW_DEPTH))
def check_seen():
visible = shm.pipe_results.heuristic_score.get()
#print(visible)
if visible > 0:
return True
else:
#print('Lost Pipe!')
return False
class Timeout(Task):
changeleds('p'), PipeQ, Finite(AbsoluteYaw(45)),
changeleds('r'), Recovery, EndRun())
Yolo = Sequential(Finite(Depth(2)), QuickTorpedoes, tobins, Bins)
FullSemifinals = Sequential(StartRun(), changeleds('g'), Gate,
changeleds('p'), tobuoy, changeleds('b'), f,
changeleds('p'), towire, changeleds('w'), Wire,
changeleds('p'), totorpedoes, QuickTorpedoes,
tobins, Bins, changeleds('r'), Recovery, EndRun())
SemifinalsNoPipe = Sequential(StartRun(), start, f, towire, Wire, Track,
EndRun())
HydroNV = Sequential(StartRun(), Finite(Depth(.4)), Finite(MoveX(12.7)),
Finite(AbsoluteYaw(30)), changeleds('r'), Recovery,
EndRun())
Hydro = Sequential(StartRun(), Gate, Finite(MoveX(3)), changeleds('r'),
Recovery, EndRun())
testled = changeleds('g')
# Transdec = Whatever()
Run = Sequential(StartRun(), Concurrent(Unsoftkill(), Semifinals))
Run1 = Recovery
Finals = Sequential(StartRun(), changeleds('g'), Finite(Depth(.7)),
Finite(MoveX(15.3)), changeleds('b'), RedBuoy,
Finite(MoveX(1.8)),
changeleds('p'), towire, changeleds('w'), Wire,
changeleds('p'), PipeQ, Finite(AbsoluteYaw(45)),
changeleds('r'), Recovery, EndRun())
def on_first_run(self, shm_group_getter, is_left, *args, **kwargs):
BOTH_DEPTH = settings.pick_up_both_depth
SEARCH_DEPTH_1 = settings.pick_up_search_depth_1
SEARCH_DEPTH_2 = settings.pick_up_search_depth_2
SEARCH_DEPTH_3 = settings.pick_up_search_depth_3
START_PICKUP_DEPTH = settings.pick_up_start_follow_depth
def downward_target_task(pt=(0, 0), deadband=(0.1, 0.1), max_out=0.04):
return DownwardTarget(
point=(lambda: shm_group_getter().center_x.get(), lambda: shm_group_getter().center_y.get()),
target=norm_to_vision_downward(*pt),
deadband=norm_to_vision_downward(-1.0 + deadband[0], -1.0 + deadband[1]),
px=0.0005,
py=0.001,
max_out=max_out,
)
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)),
)
bottom_target = (0.8, -0.2) if is_left else (-0.7, -0.4)
self.use_task(
Sequential(
VisionSelector(downward=True),
# cons(Depth(BOTH_DEPTH)),
# cons(downward_target_task(max_out=0.1)),
# stop(),
# cons(Depth(SEARCH_DEPTH_1)),
# search_at_depth(SEARCH_DEPTH_1, "top", deadband=(0.1, 0.1), depth_timeout=15),
# search_at_depth(SEARCH_DEPTH_2, "mid", deadband=(0.05, 0.05), depth_timeout=10),
search_at_depth(SEARCH_DEPTH_3, "bot", target=bottom_target, deadband=(0.05, 0.05)),
# search_at_depth(3.5, "bot", target=bottom_target, deadband=(0.05, 0.05)),
Sequential(
*(timed(Depth(depth)) for depth in np.arange(SEARCH_DEPTH_3, START_PICKUP_DEPTH + 0.1, 0.1))
),
timed(
Concurrent(
Depth(START_PICKUP_DEPTH),
Roll(7.5 * (-1 if is_left else 1)),
Pitch(-7.5),
)
),
Log("PICKING UP??"),
timed(Depth(START_PICKUP_DEPTH + 0.3)),
Sequential(
*(timed(Depth(depth), timeout=1) for depth in np.arange(START_PICKUP_DEPTH, SEARCH_DEPTH_1 + 0.1, 0.1))
),
timed(
cons(
Concurrent(
Depth(SEARCH_DEPTH_1),
Roll(0),
Pitch(0),
)
),
15
),
),
)
def on_first_run(self, shm_group, is_left, *args, **kwargs):
APPROACH_DIST = settings.drop_approach_dist
DVL_FORWARD_CORRECT_DIST = settings.drop_dvl_forward_correct_dist[is_left]
DVL_ANGLE_CORRECT = settings.drop_heading_correct[is_left]
APPROACH_FUNNEL_DEPTH = settings.approach_funnel_depth
turn_task = RelativeToInitialHeading(90 if is_left else -90)
reset_heading_task = RelativeToInitialHeading(0)
reset_heading_task()
# reset_pos_task = MoveXYRough((0, 0))
# reset_pos_task()
self.reset_pos_target = None
drop_task = FireRed if is_left else FireGreen
def record_pos():
self.reset_pos_target = (shm.kalman.north.get(), shm.kalman.east.get())
Y_DIST = -APPROACH_DIST if is_left else APPROACH_DIST
self.use_task(
Sequential(
VisionSelector(forward=True),
WithPositionalControl(
cons(Depth(APPROACH_FUNNEL_DEPTH))
),
ApproachAndTargetFunnel(shm_group),
stop(),
FunctionTask(record_pos),
VisionSelector(downward=True),
WithPositionalControl(
Sequential(
Log("Aligned"),
Log("Turning..."),
cons(turn_task),
Log("Surfacing..."),
cons(Depth(-.05)),
Log("Moving..."),
cons(
Concurrent(
MoveXYRough((DVL_FORWARD_CORRECT_DIST, Y_DIST)),
RelativeToInitialHeading(DVL_ANGLE_CORRECT),
finite=False
),
debug=True),
WithPositionalControl(
MasterConcurrent(
cons(
FunctionTask(
lambda: shm.recovery_vision_downward_red.probability.get() > .5,
finite=False
),
total=30,
success=10,
debug=True
),
VelocityY(.1 * (-1 if is_left else 1)),
),
enable=False
),
stop(),
Log("Over, dropping!..."),
drop_task(),
# Timer(3),
Log("Moving back..."),
WithPositionalControl(
Sequential(
Timed(VelocityY(-.1 * (-1 if is_left else 1)), 2),
stop(),
),
enable=False
),
cons(GoToPositionRough(lambda: self.reset_pos_target[0], lambda: self.reset_pos_target[1]), debug=True),
Log("Diving..."),
cons(Depth(.5)),
Log("Turning back..."),
cons(reset_heading_task, debug=True),
)
)
)
)
def stop():
return Concurrent(
VelocityX(0),
VelocityY(0),
)
