def on_first_run(self,
altitude,
p=0.4,
d=0.1,
deadband=0.04,
*args,
**kwargs):
rel_depth = RelativeToCurrentDepth()
def relative_offset_depth(out):
current_altitude = shm.dvl.savg_altitude.get()
rel_depth(out + math.copysign(self.RELATIVE_DEPTH_OFFSET,
current_altitude - altitude))
# Remedy for depth overshoots due to bad depth control
# TODO fix control to make this not necessary
# TODO fix Conditional to truly work as non-finite
self.go_down = PIDLoop(
shm.dvl.savg_altitude.get,
relative_offset_depth,
target=altitude,
p=p,
d=d,
negate=True,
deadband=deadband,
)
self.go_up = RelativeToCurrentDepth(-0.3)
def on_first_run(self, depth_bounds=(None, None), *args, **kwargs):
self.pid_loop_x = PIDLoop(output_function=RelativeToCurrentHeading(),
negate=True)
self.pid_loop_y = PIDLoop(output_function=RelativeToCurrentDepth(
min_target=depth_bounds[0], max_target=depth_bounds[1]),
negate=True)
self.px_default = 8
self.py_default = 0.8
def on_first_run(self, depth_bounds=(None, None), *args, **kwargs):
self.pid_loop_x = PIDLoop(output_function=VelocityY(), negate=True)
self.pid_loop_y = PIDLoop(output_function=RelativeToCurrentDepth(
min_target=depth_bounds[0], max_target=depth_bounds[1]),
negate=True)
self.px_default = 0.8
self.py_default = 0.8
PositionalControl(False)()
def on_first_run(self, vision, bin_func, *args, **kwargs):
self.use_task(ConsistentTask(PIDLoop(
input_value=lambda: bin_func().obs.length,
output_function=RelativeToCurrentDepth(),
target=1.2,
deadband=0.02,
p=0.7,
d=0.3,
)))
def on_first_run(self, vision, *args, **kwargs):
self.pid = PIDLoop(
input_value=lambda: self.y_ratio(vision),
output_function=RelativeToCurrentDepth(),
target=0.75,
deadband=constants.DEFAULT_DEADBAND,
p=2,
d=4,
negate=True,
)
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 stop(self):
RelativeToCurrentDepth(0)()
RelativeToCurrentHeading(0)()
def stop(self):
VelocityY(0)()
RelativeToCurrentDepth(0)()
def on_run(self, depth, error=0.08, *args, **kwargs):
diff = call_if_function(depth) - shm.kalman.depth.get()
relative = math.copysign(self.RELATIVE_DESIRE, diff)
RelativeToCurrentDepth(relative)()
if abs(diff) < error:
self.finish()
def DepthGrab():
return RelativeToCurrentDepth(0.3)
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)
SetActuators(on_triggers=['manipulator_release'], off_triggers=['manipulator_grab']),
Timer(0.3),
SetActuators(off_triggers=['manipulator_release']))
GrabVampireOpenCoffin = lambda: \
Sequential(
Search(visible_open),
Center(center_open, visible_open, db=20),
SetMarker('before_grab'),
Align(centerf=center_open, anglef=angle_offset_open, visiblef=visible_open),
Center(offset_open, visible_open, db=10),
MasterConcurrent(
Sequential(
Timer(15),
_Grab()),
RelativeToCurrentDepth(DESCEND_DEPTH, error=0.2),
),
Depth(SEARCH_DEPTH),
GoToMarker('before_grab'),
UnsetMarker('before_grab'),
Timeout(Consistent(visible_open, count=1.5, total=2.0, invert=False, result=True), 10),
# Grab(), # ???
# Release???
)
LID_DEPTH = 0.4
LID_DEPTH_1 = 0.5
initial_depth = 3
def record_depth():
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()
)
