def TimedLog(msg, t=0.02): return Sequential(Timer(t), Log(msg)) def TestSequential(success):
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)
def on_first_run(self, *args, **kwargs):
#heading = Heading((kalman.heading.get() + 180) % 360, error=1)
return Sequential(Pitch(0, error=1), Roll(0, error=1), Timer(1.5), Heading(lambda: kalman.heading.get() + 180, error=1), Timer(1))
def on_first_run(self, time, task, *args, **kwargs):
self.timer = Timer(time)
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, *args, **kwargs):
self.SearchTimer = Timer(3)
self.TargetTimer = Timer(4)
self.RamTimer = Timer(1)
self.PassTimer = Timer(.5)
Log('Dead reckoning through gate...'),
Timed(VelocityX(0.4), dist1),
# MoveXRough(20),
VelocityX(0),
BigDepth(1.5),
Log('Turning toward pinger tasks...'),
Heading(37 if RIGHT_HANDED else -37),
Log('Dead reckoning a little bit toward pinger tasks...'),
Timed(VelocityX(0.4), dist2),
VelocityX(0),
finite=True)
SurfaceCashIn = lambda: Sequential(
Zero(),
Log('Surfacing at cash-in'),
Timer(1.3),
BigDepth(0),
Timer(1.3),
BigDepth(1.2),
)
gate = MissionTask(
name='Gate',
cls=Gate,
modules=[shm.vision_modules.BicolorGate],
surfaces=False,
timeout=timeouts['gate'],
)
gate_dead_reckon = MissionTask(
name='GateDead',
def FireActuators(actuator_names, duration):
return Sequential(SetActuators(on_triggers=actuator_names),
Timer(duration),
SetActuators(off_triggers=actuator_names))
abs(shm.path_results.center_y.get() - v[1]) < deadband
# def on_run(self, error, p=0.0005, i=0, d=0.0, db=0.01875, max_out=0.5, negate=False, *args, **kwargs):
FollowLine = Concurrent(
PIDSway(shm.path_results.center_x.get, negate=True, db=0, p=1.2, i=.05),
While(
lambda: Sequential(
FunctionTask(lambda: shm.navigation_desires.heading.set(
(shm.kalman.heading.get() - .95 * math.degrees(
heading_sub_degrees(
math.pi / 2,
shm.path_results.angle_1.get() % math.pi, math.pi * 2))
) % 360)),
#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: True))
aaa = Sequential(Log("Started mission"), Depth(1.5), Log("At depth"),
FakeMoveX(2, .65), Zero(), Log("Starting tracking"), Timer(1),
Log("following"),
Timed(Concurrent(VelocityX(.5), FollowLine), 24),
Log("following done"), Timed(VelocityX(-.5), .1), Zero(),
Timer(1), FakeMoveY(-.6, .65), Timed(VelocityY(.5), .05),
Zero(), Timer(1), FakeMoveX(2.8, .65),
Timed(VelocityX(-.5), .1), Zero(), Timer(1), Log("turning 1"),
RelativeToInitialHeading(90), Log("turning 2"), Timer(1),
FakeMoveX(2.8, .65), Timed(VelocityX(-.5), .1), Zero(),
Timer(1), Log("turning 1"), RelativeToInitialHeading(90),
Log("turning 2"), Timer(1), FakeMoveX(2.8, .65),
Timed(VelocityX(-.5), .1), Zero(), Timer(1),
from mission.framework.combinators import Concurrent, Sequential from mission.framework.primitive import Depth, Heading, Roll from mission.framework.timing import Timer seq = Sequential(Depth(0), Heading(180), Roll(180), Timer(2)) con = Concurrent(Depth(1), Heading(90), Roll(-90), Timer(2))
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)
def on_first_run(self, *args, **kwargs):
self.timer_task = Timer(5)
self.timer_task(
) # FIXME: This is because without a run, no parameter is passed?
print("First run called with args {} and kwargs {}.".format(
args, kwargs))
def maybe_add_roll(self, original, roll_direction):
tasks = [original]
if self.roll_extension:
roll = -roll_direction * 45
tasks.extend([Roll(roll, error=10), Timer(1.0), Roll(0, error=3)])
return Sequential(*tasks)
# num here refers to the shm group, not the tracked num
SearchBuoy = lambda num, count, total: Sequential(
BigDepth(BUOY_DEPTH),
Either(
SearchFor(
ForwardSearch(forward=settings.search_forward,
stride=settings.search_stride,
speed=settings.search_speed),
shm_vars[num].visible.get,
consistent_frames=(count * 60, total * 60
) # multiple by 60 to specify in seconds
),
# Time out if we can only see one die
Sequential(
Timer(settings.search_default_zero_timeout),
Consistent(lambda: shm_vars[0].visible.get(),
count=count,
total=total,
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),
from mission.constants.timeout import timeouts
def time_left():
# TODO test this?
#time_in = Master.this_run_time - Master.first_run_time
return 0 #20 * 60 - time_in
WaitForTime = Sequential(
Zero(),
Log('Waiting until five minutes left...'),
Log('Time left: {}, so waiting {} seconds.'.format(
time_left(), max(time_left() - 5 * 60, 0))),
# Wait until five minutes left
Timer(max(time_left() - 5 * 60, 0)),
)
CASH_IN_GROUPS = [(group.center_x, group.center_y) for group in [
shm.recovery_vision_downward_bin_red,
shm.recovery_vision_downward_bin_green
]]
cash_in_center = lambda: tuple(
sum([val.get() for val in dimen]) for dimen in CASH_IN_GROUPS)
# TODO test this
SurfaceAtCashIn = Sequential(
Zero(),
Timer(3),
Log('Aligning with cash-in'),
def __init__(self,
location_validator,
target_coordinates,
vision_group,
collision_validator,
ram_concurrent_task=NoOp(),
first_buoy=False,
right=True,
yellow=False,
*args,
**kwargs):
super().__init__(*args, **kwargs)
self.logv("Starting {} task".format(self.__class__.__name__))
self.location_validator = location_validator
self.target_coordinates = target_coordinates
self.collision_validator = collision_validator
self.ram_concurrent_task = ram_concurrent_task
self.heading_task = HeadingRestore()
if first_buoy:
self.locator_task_a = LocateAlignedBuoy(self.location_validator,
forward=True)
else:
self.locator_task_a = LocateAdjacentBuoy(self.location_validator,
right=right)
self.locator_task_b = LocateAlignedBuoy(self.location_validator,
forward=False)
self.align_task = AlignTarget(self.location_validator,
self.locator_task_a,
self.target_coordinates, vision_group,
self.heading_task)
self.ram_task = RamTarget(
self.location_validator,
self.collision_validator,
#LocateBuoy(self.location_validator,checkBehind=True),
self.locator_task_b,
self.ram_concurrent_task)
if yellow:
self.forward_task = DirectionalSurge(6, .5)
self.retreat_task = DirectionalSurge(4, -.5)
else:
self.forward_task = DirectionalSurge(constants.FORWARD_TIME, .2)
self.retreat_task = DirectionalSurge(constants.BACKUP_TIME, -1)
self.depth_task = DepthRestore()
self.tasks = Sequential(
Zero(),
SeqLog("Restoring Depth"),
self.depth_task,
SeqLog("Locating Buoy"),
self.locator_task_a,
SeqLog("Aligning"),
self.align_task,
SeqLog("Approaching"),
self.ram_task,
Zero(),
Timer(.5),
)
#self.tasks = Sequential(Zero(), self.depth_task, self.locator_task, self.align_task,
# self.ram_task, self.forward_task, self.retreat_task,
# self.heading_task)
self.TIMEOUT = 90
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 __init__(self,
location_validator,
target_coordinates,
vision_group,
collision_validator,
ram_concurrent_task=NoOp(),
first_buoy=False,
right=True,
yellow=False,
*args,
**kwargs):
"""
location_validator - a function that returns True when the target has
been found and False otherwise
target_coordinates - a tuple representing the coordinates of the target
in the xz-plane
vision_group - the shm group for the buoy
collision_validator - a function that returns True when there has been a
collision with the target and False otherwise.
ram_concurrent_task - an optional task to run concurrently when ramming
the target
"""
super().__init__(*args, **kwargs)
self.logv("Starting {} task".format(self.__class__.__name__))
self.location_validator = location_validator
self.target_coordinates = target_coordinates
self.collision_validator = collision_validator
self.ram_concurrent_task = ram_concurrent_task
self.heading_task = HeadingRestore()
if first_buoy:
self.locator_task_a = LocateAlignedBuoy(self.location_validator,
forward=True)
else:
self.locator_task_a = LocateAdjacentBuoy(self.location_validator,
right=right)
self.locator_task_b = LocateAlignedBuoy(self.location_validator,
forward=False)
self.align_task = AlignTarget(self.location_validator,
self.locator_task_a,
self.target_coordinates, vision_group,
self.heading_task)
self.ram_task = RamTarget(
self.location_validator,
self.collision_validator,
#LocateBuoy(self.location_validator,checkBehind=True),
self.locator_task_b,
self.ram_concurrent_task)
if yellow:
self.forward_task = DirectionalSurge(6, .5)
self.retreat_task = DirectionalSurge(3, -.5)
else:
self.forward_task = DirectionalSurge(constants.FORWARD_TIME, .2)
self.retreat_task = DirectionalSurge(constants.BACKUP_TIME, -1)
self.depth_task = DepthRestore()
self.tasks = Sequential(Zero(),
SeqLog("Restoring Depth"), self.depth_task,
SeqLog("Locating Buoy"), self.locator_task_a,
SeqLog("Aligning"), self.align_task,
SeqLog("Approaching"), self.ram_task, Zero(),
Timer(.5),
SeqLog("Ramming"), self.forward_task,
SeqLog("Backing up"), self.retreat_task,
SeqLog("Restoring heading"), self.heading_task)
#self.tasks = Sequential(Zero(), self.depth_task, self.locator_task, self.align_task,
# self.ram_task, self.forward_task, self.retreat_task,
# self.heading_task)
self.TIMEOUT = 90
set_gate = lambda: MissionTask(name="SetGate",
cls=lambda: SetMarker('gate'),
modules=[],
surfaces=False,
timeout=20)
goto_gate = lambda: MissionTask(name="GoToGate",
cls=lambda: GoToMarker('gate'),
modules=[],
surfaces=False,
timeout=60)
path = lambda: MissionTask(
name="path",
cls=lambda: Sequential(Timer(5), RelativeToInitialHeading(-45), Timer(5),
MoveX(8, deadband=0.2)),
modules=[],
surfaces=False,
timeout=30)
tasks_nonrandom = [
lambda: gate,
path,
set_gate,
track_pinger,
lambda: surface,
goto_gate,
search_torpedoes,
]
def on_first_run(self, seconds, function, value, *args, **kwargs):
self.timer = Timer(seconds)
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(
Log('Aligning'),
MasterConcurrent(
Consistent(lambda: close_to(centerf(), CAM_CENTER) and abs(anglef()) < aligndb, count=2.3, total=3, invert=False, result=True),
While(lambda: Consistent(visiblef, count=2.5, total=3.0, invert=True, result=False), True),
While(lambda: Center(centerf, visiblef), True),
PIDHeading(anglef, p=0.47),
AlwaysLog(lambda: 'align %s' % str(anglef()))),
Zero())
_Grab = lambda: SetActuators(on_triggers=['manipulator_grab'])
_Release = lambda: Sequential(
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),
),
get_wolf_center, lambda: heading_to_vector(shm.bins_status.wolf_angle.get(
)), shm.bins_status.wolf_angle.get, shm.bins_status.wolf_visible.get, math.
pi / 2, -.2j, DEADBAND)
center_bat = lambda: PipeAlign(
get_bat_center, lambda: heading_to_vector(shm.bins_status.bat_angle.get(
)), shm.bins_status.bat_angle.get, shm.bins_status.bat_visible.get, math.pi
/ 2, -.02 - .15j, DEADBAND)
cb = center_bat()
full_wolf = lambda: Sequential(
Log("Targeting wolf for drop"),
Depth(1),
center_wolf(),
Depth(2.3),
center_wolf(),
Timer(.4),
FireActuator('right_marker', 0.25),
FireActuator('left_marker', 0.25),
# do dropper stuff
)
full_bat = lambda: Sequential(
Log("Targeting bat for drop"),
Depth(1),
center_bat(),
Depth(2.3),
center_bat(),
Timer(.4),
FireActuator('right_marker', 0.25),
FireActuator('left_marker', 0.25),
# do dropper stuff
def startup():
shm.switches.hard_kill.set(0)
shm.switches.soft_kill.set(0)
def shutdown():
shm.switches.soft_kill.set(1)
Qualify = Sequential(
RelativeToInitialHeading(0),
FunctionTask(startup),
Depth(1),
Timer(2),
Depth(2),
Timer(2),
Depth(2.5),
Timer(2),
# Castor
InterMoveX(8),
Depth(0.8),
InterMoveX(7),
# # Pollux
# InterMoveX(3),
# Depth(1.5),
# InterMoveX(4),
RelativeToInitialHeading(90),
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()))
)
def tasks_from_params(task, params, tup=False):
return [task(*param if tup else param) for param in params]
def tasks_from_param(task, param, length, tup=False):
return [task(*param if tup else param) for i in range(length)]
def interleave(a, b):
return [val for pair in zip(a, b) for val in pair]
Descend = lambda depth, timeout, pause, deadband: Sequential(
Either(
Timer(timeout),
Depth(depth, deadband=deadband),
),
Timer(pause),
)
# This lets us descend in depth steps rather than all at once
class BigDepth(Task):
def on_first_run(self,
depth,
largest_step=0.5,
timeout=4,
pause=0,
deadband=0.1):
init_depth = shm.kalman.depth.get()
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 on_first_run(self, task_class, *args, **kwargs):
self.vision = Vision()
task = task_class(self.vision, *args, **kwargs)
self.task = Sequential(Timer(1), task)
from mission.framework.task import Task
from mission.framework.combinators import Sequential
from mission.framework.timing import Timer
from mission.framework.primitive import FunctionTask, Log
import shm
from mission.constants.sub import PISTONS, TORPEDOES, PISTON_DELAY, TORPEDO_DELAY
Actuate = lambda channel, value: FunctionTask(lambda: channel.set(value))
FirePiston = lambda piston: Sequential(
# Open
Actuate(piston[1], 0),
Actuate(piston[0], 1),
Timer(PISTON_DELAY),
# Close
Actuate(piston[0], 0),
Actuate(piston[1], 1),
Timer(PISTON_DELAY),
# Reset
Actuate(piston[1], 0),
Timer(PISTON_DELAY),
)
FireTorpedo = lambda torpedo: Sequential(
# Fire
Actuate(torpedo, 1),
Timer(TORPEDO_DELAY),
# Reset
Actuate(torpedo, 0),
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))
focus_left = lambda: focus_elem(lambda: shm.gate.leftmost_x)
focus_middle = lambda: focus_elem(lambda: shm.gate.middle_x)
def TestTimeout():
return Timeout(Timer(2), 1)