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_run(self, target, *args, **kwargs):
current = shm.kalman.heading.get()
diff = heading_sub_degrees(target, current)
inc = [-1, 1][diff > 0] * 12
RelativeToCurrentHeading(inc)()
if abs(diff) < 5:
self.finish()
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 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, vision, *args, **kwargs):
self.pid = PIDLoop(
input_value=lambda: x_ratio(vision),
output_function=RelativeToCurrentHeading(),
target=0.5,
deadband=.09,
p=50,
negate=True,
)
def on_run(self, vision, *args, **kwargs):
# Try to center on entire gate first
if vision.bottom is None:
self.finish(success = False)
return
if vision.bottom is not None or \
(vision.left is not None and vision.right is not None):
self.pid()
elif vision.left is not None: # Otherwise try to find other vertical bar
RelativeToCurrentHeading(1)()
elif vision.right is not None:
RelativeToCurrentHeading(-1)()
else:
self.finish()
if self.pid.finished:
self.finish()
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, *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 on_first_run(self, *args, **kwargs):
self.pid_loop_heading = PIDLoop(output_function=RelativeToCurrentHeading(), modulo_error=True)
def stop(self):
RelativeToCurrentDepth(0)()
RelativeToCurrentHeading(0)()
def on_first_run(self, *args, **kwargs):
self.rel_curr_heading = RelativeToCurrentHeading()
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()
)
),
Conditional(
main_task=FunctionTask(lambda: gate_elems() < 3),
on_success=Sequential(
Log('cannot see all gate_elem'),
Fail()
),
on_fail=Log('is aligned to 3 elems')
),
from mission.framework.task import Task from mission.framework.movement import RelativeToCurrentHeading spin_speed = 90 spin_right = RelativeToCurrentHeading(spin_speed) spin_left = RelativeToCurrentHeading(-spin_speed)
def on_first_run(self, *args, **kwargs):
self.pid_loop = PIDLoop(output_function=RelativeToCurrentHeading())