def on_first_run(self, *args, **kwargs):
self.killed = shm.switches.hard_kill.get()
self.use_task(
Sequential(
VisionFramePeriod(0.1), # reset this
FunctionTask(lambda: shm.switches.soft_kill.set(1)),
FunctionTask(lambda: shm.deadman_settings.enabled.set(False)),
Log('Disabling Record vision module'),
FunctionTask(lambda: shm.vision_modules.Record.set(0)),
#AllLeds('orange'),
#Log('Wating for alignment...'),
#WaitForUnkill(wait=1.0),
#ZeroHeading(),
#Log('Aligned heading!'),
#AllLeds('cyan'),
# Need a swimmer to do this
Log('Waiting for re-kill...'),
WaitForUnkill(killed=False, wait=0.5),
#AllLeds('blue'),
Log('Waiting for unkill signal to start mission...'),
WaitForUnkill(wait=5.0),
Log('Starting mission!'),
#AllLeds('red'),
Log('Zeroing'),
ZeroHeading(),
Zero(),
FunctionTask(lambda: shm.switches.soft_kill.set(0)),
EnableController(),
Heading(0), # This will revert to the aligned heading
Log('Enabling Record vision module'),
FunctionTask(lambda: shm.vision_modules.Record.set(1)),
))
def on_first_run(self, *args, **kwargs):
self.killed = shm.switches.hard_kill.get()
self.use_task(
Sequential(
FunctionTask(lambda: shm.switches.soft_kill.set(1)),
FunctionTask(lambda: shm.deadman_settings.enabled.set(False)),
Log('Disabling Record vision module'),
FunctionTask(lambda: shm.vision_modules.Record.set(0)),
WaitForUnkill(killed=False, wait=.5),
WaitForUnkill(),
Log('Zeroing'),
Zero(),
FunctionTask(lambda: shm.switches.soft_kill.set(0)),
EnableController(),
Heading(0),
Log('Enabling Record vision module'),
FunctionTask(lambda: shm.vision_modules.Record.set(1)),
))
def on_first_run(self, tasks, *args, **kwargs):
#tasks.insert(0, BeginMission)
tasks.append(EndMission)
self.use_task(Retry(lambda: Sequential(
RunTask(BeginMission),
Concurrent(
Sequential(subtasks=[RunTask(t) for t in tasks],),
Fail(WaitForUnkill(killed=False, wait=1)),
),
), float('inf'))
)
def make(task):
if killed:
return Sequential(WaitForUnkill(), task)
else:
return task
PipeToWire,
Wire,
#Pinger
]
after_unkill = lambda task: HardkillGuarded(
Sequential(
# Because WaitForUnhardKill should grab heading when sub switched on.
#ZeroWithoutHeading(),
Zero(),
#FunctionTask(lambda: set_zero_heading()),
task,
#Depth(0.0)
))
Run = lambda task: Sequential(EnableController(), WaitForUnkill(),
after_unkill(task))
Full = Run(Sequential(
# gate,
Opt(tasks=tasks, restrictions=restrictions), ))
defaultOrdering = tasks
killed = shm.switches.hard_kill.get()
if killed:
print('HARD KILLED - mission will WAIT for unkill!')
else:
print('NOT hard killed, running mission!')