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 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, vision, *args, **kwargs):
self.pid = PIDLoop(
input_value=lambda: bbar_width_ratio(vision),
output_function=VelocityX(),
target= constants.BBAR_RATIO_TARGET,
deadband=constants.DEFAULT_DEADBAND,
p=1,
d=4
)
self.retreat_task = Sequential(Log('Retreat'), Timed(VelocityX(-.3), 1))
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, 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)
import shm
results_groups = shm.bicolor_gate_vision
class Consistent(Task):
def on_first_run(self, test, count, total, invert, result):
# Multiple by 60 to specify in seconds
self.checker = ConsistencyCheck(count * 60, total * 60, default=False)
def on_run(self, test, count, total, invert, result):
test_result = call_if_function(test)
if self.checker.check(not test_result if invert else test_result):
self.finish(success=result)
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
Zero(),
finite=False
)
align_on_three_elem = lambda:\
Sequential(
Log('aligning on three elems'),
ConsistentTask(
# while we CAN see all gates
FinishIf(
task=Concurrent(
# align leftmost and rightmost by length
PIDLoop(
input_value=lambda: shm.gate.rightmost_len.get() / shm.gate.leftmost_len.get(),
target=1,
p=0.5,
deadband=0.1,
output_function=VelocityY()
),
# align to the center of the leftmost and rightmost
PIDLoop(
input_value=lambda: (shm.gate.leftmost_x.get() + shm.gate.rightmost_x.get()) / 2,
target=lambda: shm.gate.img_width.get() / 2,
p=0.2,
deadband=alignment_tolerance_fraction,
output_function=RelativeToCurrentHeading(),
negate=True
),
finite=False
),
condition=lambda: gate_elems() < 3 or is_aligned()
def on_first_run(self, *args, **kwargs):
self.pid_loop = PIDLoop(output_function=RelativeToCurrentHeading())
def on_first_run(self, *args, **kwargs):
self.pid_loop = PIDLoop(output_function=VelocityX())
def firstrun(self, *args, **kwargs):
print("forward started")
self.pid_loop_x = PIDLoop(output_function=VelocityX())
self.total_distance = 0
def firstrun(self, *args, **kwargs):
self.pid_loop_y = PIDLoop(output_function=VelocityY())
self.total_distance = 0
