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 on_first_run(self, vision, *args, **kwargs):
table = shm.recovery_world_table.get()
def record_position():
table.know_pos = True
table.north = shm.kalman.north.get()
table.east = shm.kalman.east.get()
shm.recovery_world_table.set(table)
self.use_task(
Sequential(
# Move to the table depth first so we don't blow things off
Log('Moving to table depth'),
FastDepth(constants.table_depth),
Sequential(
Log('Returning to table position ({}, {})'.format(
table.north, table.east)),
GoToPositionRough(table.north, table.east, optimize=False),
) if table.know_pos else NoOp(),
Log('Searching for table'),
MasterConcurrent(
IdentifyObjects(lambda: vision.ellipses, 2),
SearchWithGlobalTimeout(timeout=self.TIMEOUT)),
Log('Centering table'),
CenterCentroid(lambda: vision.coords(vision.ellipses)),
Zero(),
Log('Recording table position'),
FunctionTask(record_position),
))
def Align(centerf, alignf, visiblef, px=0.14, py=0.004, p=0.009, dx=0.00, dy=0.000, dsway=0.00, db=0): # dx=0.005, dy=0.0005, dsway=0.002
return MasterConcurrent(
Consistent(lambda: close_to(centerf(), CAM_CENTER) and aligned(alignf()), count=1.5, total=2, invert=False, result=True),
Consistent(visiblef, count=1.5, total=2.0, invert=True, result=False),
HeadingTarget(point=centerf, target=CAM_CENTER, px=px, py=py, dy=dy, dx=dx, deadband=(db,db)),
PIDSway(alignf, p=p, d=dsway, db=db),
AlwaysLog(lambda: "align_h: %d" % (alignf(),)))
def on_first_run(self, clockwise=True, steps=5, spins=2, *args, **kwargs):
delta_heading = [-1, 1][clockwise] * 45
subspins = [MasterConcurrent(CheckSpinCompletion(2),
RelativeToCurrentHeading(delta_heading))]
self.use_task(Sequential(
Timed(VelocityX(1), 1), Zero(),
MasterConcurrent(
Sequential(subtasks=subspins),
VelocityX(lambda: 2 * math.cos(math.radians(shm.kalman.heading.get()))),
VelocityY(lambda: 10 * math.sin(math.radians(shm.kalman.heading.get()))),
),
Zero(),
))
def on_first_run(self, speed=40, db=7, target=180, *args, **kwargs):
init_heading = None
def set_init_heading():
nonlocal init_heading
init_heading = shm.kalman.heading.get()
return True
set_init_heading()
self.use_task(
# TODO: DO WE NEED THE WHILE OR DO WE WANT IT TO TERMINATE
# While(lambda: \
Sequential(
RelativeToInitialHeading(speed),
MasterConcurrent(
FunctionTask(lambda: (abs(shm.desires.heading.get() - (init_heading + target)) % 360) < db, finite=False),
RelativeToCurrentHeading(speed)),
# Move back a bit, we might be too close
# TODO: DO WE EVEN NEED THIS?
# MoveX(-1),
# Succeed(FunctionTask(set_init_heading))
)
# , True),
)
def ApproachCenterSize(sizef, centerf, alignf, visiblef, size_thresh, p=0.000003, px=0.0009, py=0.005, dx=0.00, dy=0.01, d=0, consistent_total=2.0, closedb=20, db=30000):
return MasterConcurrent(
Consistent(lambda: abs(sizef()-size_thresh) < db and close_to(centerf(), CAM_CENTER, db=closedb), count=1.7, total=2.0, invert=False, result=True),
Consistent(visiblef, count=1.3, total=consistent_total, invert=True, result=False),
While(lambda: Center(centerf, visiblef, px=px, py=py, dx=dx, dy=dy), True),
PIDStride(lambda: sizef()-size_thresh, p=p, d=d),
AlwaysLog(lambda: "center: {}, target: {}, size{}".format(CAM_CENTER, centerf(), sizef())))
def ApproachAlignSize(sizef, centerf, alignf, visiblef, size_thresh, db=30000):
return MasterConcurrent(
Consistent(lambda: abs(sizef()-size_thresh) < db and aligned(alignf(), db=5), count=1.3, total=2.0, invert=False, result=True),
Consistent(visiblef, count=1.3, total=2.0, invert=True, result=False),
While(lambda: Align(centerf, alignf, visiblef), True),
PIDStride(lambda: sizef()-size_thresh),
AlwaysLog(lambda: "center: {}, target: {}, align: {}, size{}".format(CAM_CENTER, centerf(), alignf(), sizef())))
def on_first_run(self, task, time, *args, **kwargs):
# Client can check if task timed out with
# timout.timer.has_ever_finished
self.timer = Timer(time)
self.use_task(MasterConcurrent(
task,
Fail(Sequential(
self.timer,
Log('{} timed out!'.format(task.__class__.__name__)),
)),
))
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 on_first_run(self, vision, *args, **kwargs):
tower_tubes = get_shm_array(shm.recovery_world_tower, 'has_tube')
search_tubes = 2 if sum(tower_tubes) == len(constants.colors) else 1
tower = shm.recovery_world_tower.get()
go_to_tower = NoOp()
if tower.know_pos:
go_to_tower = Sequential(
Log('Returning to tower position ({}, {})'.format(
tower.north, tower.east)),
GoToPositionRough(tower.north, tower.east, optimize=False),
)
if sum(tower_tubes) > 0:
search_tower = Sequential(
Log('Searching for tower'),
FastDepth(constants.tower_depth),
MasterConcurrent(
IdentifyObjects(
lambda: vision.tubes,
search_tubes,
),
SearchWithGlobalTimeout(),
),
)
else:
search_tower = Log('Not searching for tower, no tubes on tower')
if sum(tower_tubes) > 0:
center_tower = Sequential(
Log('Centering tower'),
CenterCentroid(lambda: vision.coords(vision.tubes)),
Zero(),
)
else:
center_tower = Log('No tubes on tower, not centering')
self.use_task(
Sequential(
# Go to depth early so we don't hit the tower
Log('Going to default tower depth of {}'.format(
constants.tower_depth)),
FastDepth(constants.tower_depth),
go_to_tower,
search_tower,
center_tower,
))
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(),
)))
shm.recovery_crucifix.offset_y.get())
def angle_offset_crucifix():
return shm.recovery_crucifix.angle_offset.get()
def size_crucifix():
return shm.recovery_crucifix.size.get()
GrabCrucifix = lambda: \
Sequential(
Search(visible_crucifix),
Center(center_crucifix, visible_crucifix),
Align(center_crucifix, angle_offset_crucifix, visible_crucifix),
Center(offset_crucifix, visible_crucifix, db=10),
MasterConcurrent(
Sequential(
Timer(15),
_Grab()),
RelativeToCurrentDepth(DESCEND_DEPTH, error=0.2),
),
Depth(SEARCH_DEPTH, error=0.2),
Timeout(Consistent(visible_crucifix, count=1.5, total=2.0, invert=True, result=True), 10),
Log('crucifix grabbed successfully'),
)
SearchCrucifix = lambda: Search(visible_crucifix)
CenterCrucifix = lambda: Center(center_crucifix, visible_crucifix)
self.progress = 0
def on_run(self, N):
new_angle = shm.kalman.roll.get()
angle_diff = new_angle - self.current_r
self.current_r = new_angle
Ad = ((angle_diff + 180) % 360) - 180
self.progress += Ad if N > 0 else -Ad
if self.progress > abs(N): self.finish()
pv = shm.settings_roll.kP.get()
roll = lambda: Sequential(
FunctionTask(lambda: shm.settings_roll.kP.set(.6)),
#*([RelativeToInitialRoll(90)] * 8),
#Timed(RelativeToCurrentRoll(90), 6),
MasterConcurrent(RollDegrees(720 - 45), RelativeToCurrentRoll(90),
VelocityX(.35)),
#MasterConcurrent(RollDegrees(360), RelativeToCurrentRoll(90), VelocityX(.35)),
#Timer(.5),
#MasterConcurrent(RollDegrees(360 - 45), RelativeToCurrentRoll(90), VelocityX(.35)),
#MasterConcurrent(RollDegrees(-720 + 45), RelativeToCurrentRoll(-90)),
FunctionTask(lambda: shm.settings_roll.kP.set(pv)),
Concurrent(
Roll(0),
VelocityX(0),
),
Timer(2),
)
t2 = Timed(RelativeToCurrentRoll(-90), 3)
HeadingTarget(point=centerf, target=CAM_CENTER, px=px, py=py, dy=dy, dx=dx, deadband=(db,db)),
PIDSway(alignf, p=p, d=dsway, db=db),
AlwaysLog(lambda: "align_h: %d" % (alignf(),)))
# def AlignBoard():
# return Align(board_center, align_h, visible)
# def AlignHeart():
# return Align(heart, align_h, visible)
# def AlignLeftHole():
# return Align(left_hole, align_h, visible)
# def AlignRightHole():
# return Align(right_hole, align_h, visible)
Center = lambda centerf, visiblef, targetf=CAM_CENTER, px=0.0006, py=0.003, dx=0.000, dy=0.0, db=0, iy=0.0002, closedb=5: MasterConcurrent(
Consistent(lambda: close_to(centerf(), targetf, db=closedb), count=4.0, total=5.0, invert=False, result=True),
Consistent(visiblef, count=1.5, total=2.0, invert=True, result=False),
ForwardTarget(point=centerf, target=targetf, px=px, py=py, dx=dx, dy=dy, iy=iy, deadband=(db,db)),
AlwaysLog(lambda: "center: {}, target: {}".format(targetf, centerf())))
# @withApproachHeartOnFail
# def CenterHeart():
# return Center(centerf=heart, visiblef=visible, py=0.002, closedb=20)
# def CenterBelt():
# return Center(centef=belt, visiblef=visible, closedb=15)
# @withApproachLeftHoleOnFail
# def CenterLeftHole():
# return Center(centerf=left_hole, visiblef=visible, closedb=15)
# @withApproachRightHoleOnFail
# def CenterRightHole():
# return Center(centerf=right_hole, visiblef=visible, closedb=15)
@withApproachAlignOnFail
def PipeAlign(get_center,
heading_vect,
heading,
get_visible,
trg,
dst,
deadband,
angle_range=math.pi * 2,
abs_offset=(0, 0)):
return Sequential(
Log("PipeAlign start"),
MasterConcurrent(
Consistent(lambda: is_done(heading_vect, heading, get_center, trg,
dst, deadband, abs_offset),
count=.5,
total=.75,
invert=False,
result=True),
While(
lambda: Sequential(
#Log("attempt asdasd"),
Concurrent(
DownwardTarget(
lambda: get_center(),
target=lambda:
(np.float64([heading_vect()]).view(np.complex128) *
-dst).view(np.float64)[0] + abs_offset,
deadband=(deadband, deadband),
px=2,
py=2,
dx=.5,
dy=.5 #, ix=.15, iy=.15
),
While(
lambda: Sequential(
FunctionTask(
lambda: shm.navigation_desires.heading.set(
(shm.kalman.heading.get(
) - .95 * math.degrees(
heading_sub_degrees(
trg, heading(), angle_range))
) % 360) if get_visible() 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()))),
Timer(.05)),
lambda: abs(
heading_sub_degrees(trg, heading(), math.pi * 2
)) > math.radians(5)
) #, count=6, total=8, invert=False, result=True),
)),
lambda:
True #lambda: abs(heading_sub_degrees(trg, heading(), math.pi*2)) > math.radians(5) or abs(shm.path_results.center_x.get()) > .08 or abs(shm.path_results.center_y.get()) > .08
),
#, count=3, total=4, invert=False, result=True),
#While(lambda: Log("V: {} h: {} d: {} x: {} y: {} c: {}".format(shm.path_results.num_lines.get(), heading(), math.degrees(heading_sub_degrees(trg, heading(), math.pi*2)), shm.path_results.center_x.get(), shm.path_results.center_y.get(), heading_to_vector(heading()) * dst)), lambda: True),
#While(lambda: Log(s(int(math.degrees(heading_sub_degrees(trg, heading(), math.pi*2))) + 180)), lambda: True),
#While(lambda: Log(s2(int(math.degrees(trg)), int(math.degrees(heading()))) if shm.path_results.num_lines.get() == 2 else 'X'), lambda: True),
),
Log("Centered on Pipe in PipeAlign!"),
#FunctionTask(lambda: shm.navigation_desires.heading.set(-180/math.pi*heading()+shm.kalman.heading.get()))
)
)
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(),
depth,
largest_step=0.5,
timeout=4,
pause=0,
deadband=0.1):
init_depth = shm.kalman.depth.get()
steps = math.ceil(abs(depth - init_depth) / largest_step)
depth_steps = interpolate_list(init_depth, depth, steps)
params = [(step, timeout, pause, deadband) for step in depth_steps]
#self.use_task(Sequential(*interleave(tasks_from_params(Depth, depth_steps), tasks_from_param(Timer, timeout, length=steps))))
self.use_task(
Sequential(*tasks_from_params(Descend, params, tup=True)))
FakeMoveX = lambda dist, speed: Sequential(
MasterConcurrent(Timer(abs(dist / speed)),
VelocityX(-speed if dist < 0 else speed)),
MasterConcurrent(Timer(0.2), VelocityX(speed if dist < 0 else -speed)),
MasterConcurrent(Timer(0.2), VelocityX(0)),
)
FakeMoveY = lambda dist, speed: Sequential(
MasterConcurrent(Timer(abs(dist / speed)),
VelocityY(-speed if dist < 0 else speed)),
MasterConcurrent(Timer(0.2), VelocityY(speed if dist < 0 else -speed)),
MasterConcurrent(Timer(0.2), VelocityY(0)),
)
# A version of VelocitySwaySearch, but better.
# We use this for minisub because it doesn't drift as much.
class ForwardSearch(Task):
#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),
XTarget(x=results_groups.gate_center_x.get, db=0.05),
finite=False,
)),
Log('Charging...'),
Timed(VelocityX(0.5 if is_mainsub() else 0.2), settings.charge_dist),
Log('Through gate!'),
)
visiblef=single_buoy_visible,
px=0.24,
py=0.004,
dx=0.3,
dby=30)
# Centers the buoy using forward target
CenterBuoy = lambda centerf, visiblef, px=0.007, py=0.006, d=0.005, db=0: MasterConcurrent(
Consistent(lambda: close_to(centerf(), CAM_CENTER),
count=2.7,
total=3.0,
invert=False,
result=True),
#Consistent(visiblef, count=0.2, total=0.3, invert=True, result=False),
ForwardTarget(point=centerf,
target=CAM_CENTER,
px=px,
py=py,
dx=d,
dy=d,
deadband=(db, db)),
AlwaysLog(lambda: "center: {}, target: {}".format(CAM_CENTER, centerf())))
# Centers any side of the triangular buoy
CenterAnyBuoy = lambda: CenterBuoy(centerf=any_buoy_center,
visiblef=triangle_visible)
# Centers only the called side
CenterCalledBuoy = lambda: CenterBuoy(centerf=call_buoy_center,
visiblef=call_buoy_visible)
# Centers the single target buoy
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()
)
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,
count=2.3,
total=3,
invert=False,
result=True), Depth(depth)), # TODO: BigDepth?
Zero())
close_to = lambda point1, point2, db=10: abs(point1[0] - point2[
MasterConcurrent(
While(
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
) #lambda: abs(heading_sub_degrees(trg, heading(), math.pi*2)) > math.radians(5) or abs(shm.path_results.center_x.get()) > .08 or abs(shm.path_results.center_y.get()) > .08
),
#, count=3, total=4, invert=False, result=True),
#While(lambda: Log("V: {} h: {} d: {} x: {} y: {} c: {}".format(shm.path_results.num_lines.get(), heading(), math.degrees(heading_sub_degrees(trg, heading(), math.pi*2)), shm.path_results.center_x.get(), shm.path_results.center_y.get(), heading_to_vector(heading()) * dst)), lambda: True),
#While(lambda: Log(s(int(math.degrees(heading_sub_degrees(trg, heading(), math.pi*2))) + 180)), lambda: True),
#While(lambda: Log(s2(int(math.degrees(trg)), int(math.degrees(heading()))) if shm.path_results.num_lines.get() == 2 else 'X'), lambda: True),
),
def on_first_run(self, task_func, *args, **kwargs):
vision = Vision()
self.use_task(
MasterConcurrent(task_func(vision, *args, **kwargs), vision))
result=True,
invert=False),
),
),
)
BackUpUntilVisible = lambda num, speed, timeout: Conditional(
Defer(
main_task=Either(
Sequential(
# Get at least a little bit away first
FakeMoveX(dist=-0.3, speed=0.2),
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'),
),
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),
While(lambda: DownwardTarget(point=centerf, target=CAM_CENTER, deadband=(0, 0), px=px, py=py), True),
AlwaysLog(lambda: 'center = {}, target = {}'.format(centerf(), CAM_CENTER))))
CenterAny = lambda: Center(center_any, any_visible)
# 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, count=2.3, total=3, invert=False, result=True),
# Depth(depth)), # TODO: BigDepth?
# Zero())
close_to = lambda point1, point2, db=20: abs(point1[0]-point2[0]) < db and abs(point1[1]-point2[1]) < db
Align = lambda centerf, anglef, visiblef, closedb=10, aligndb=7: Sequential(
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 on_run(self, task, condition, **kwargs):
success = condition()
if success:
self.finish(success=success)
else:
self.finished = False
task()
pv = shm.settings_roll.kP.get()
rolly_roll = lambda:\
Sequential(
FunctionTask(lambda: shm.settings_roll.kP.set(.6)),
MasterConcurrent(
RollDegrees(360 * 2 - 180),
RelativeToCurrentRoll(90),
VelocityX(0)
),
Timer(1),
FunctionTask(lambda: shm.settings_roll.kP.set(pv)),
Roll(0, error=10)
)
def focus_elem(elem_x, offset=0):
return HeadingTarget(
point=[lambda: elem_x().get(), 0],
target=lambda: [shm.gate.img_width.get() / 2 + offset, 0],
px=0.3,
deadband=(20, 1))
def fake_move_y(d):
v = 0.2
if d < 0:
d *= -1
v *= -1
return Sequential(MasterConcurrent(Timer(d / v), VelocityY(v)), 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 = 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),
)))
self._finish()
return
self.depth()
class ToPipe(FiniteTask):
def firstrun(self):
self.start = time.time()
self.count = time.time()
def run(self):
if shm.pipe_results.heuristic_score.get() < 3000:
self.start = time.time()
if self.this_run - self.start > .3 or self.this_run - self.count > 60:
self._finish()
class Pause(FiniteTask):
def firstrun(self, wait=0):
self.start = time.time()
def run(self, wait=0):
if self.this_run - self.start > wait:
self._finish()
Gate = Sequential(dive(), MasterConcurrent(ToPipe(), VelocityX(.4)),
VelocityX(0))
