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 Classify():
return VisionTask(lambda vision: Sequential(
FunctionTask(lambda: shm.vision_modules.Debug.set(1)),
Timer(2),
FunctionTask(vision.classify_tubes),
Infinite(Timer(1)),
))
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.use_task(Sequential(
Log('Ending Run! Surfacing and softkilling'),
Zero(),
Depth(0),
FunctionTask(lambda: shm.switches.soft_kill.set(1)),
FunctionTask(lambda: shm.deadman_settings.enabled.set(True)),
))
def WithQuaternionControl(task):
v = shm.settings_control.quat_pid
val = v.get()
return Defer(
Sequential(FunctionTask(lambda: v.set(True)), task),
FunctionTask(lambda: v.set(val)),
)
def ConfigureHydromath(enable, gain_high=True):
if os.environ["CUAUV_VEHICLE"] == "pollux":
return Sequential(
FunctionTask(lambda: shm.hydrophones_settings.enabled.set(enable)),
FunctionTask(p_step))
else:
return Sequential(
FunctionTask(lambda: shm.hydrophones_settings.enabled.set(enable)),
FunctionTask(set_gain if gain_high else set_gain_12))
def NavigationSpeed(task, speed):
speed_var = shm.navigation_settings.max_speed
init_speed = speed_var.get()
return Defer(
Sequential(
FunctionTask(lambda: speed_var.set(speed)),
task,
),
FunctionTask(lambda: speed_var.set(init_speed)),
)
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 TestAll():
class Inc:
def __init__(self):
self.failed_tests = 0
def inc(self):
Log('BAD!')()
self.failed_tests += 1
def log(self):
Log('Failed tests: {}'.format(self.failed_tests))()
inc = Inc()
def IncFailed(t): return Conditional(t, on_fail=FunctionTask(inc.inc))
tasks = [
GoodSequential(),
InvertSuccess(BadSequential()),
InvertSuccess(TestRetry()),
GoodConcurrent(),
InvertSuccess(BadConcurrent()),
InvertSuccess(TestMasterConcurrent1()),
InvertSuccess(TestMasterConcurrent2()),
TestMasterConcurrent3(),
InvertSuccess(TestMasterConcurrent4()),
InvertSuccess(TestMasterConcurrent5()),
InvertSuccess(TestTimeout()),
TestSubtask1(),
InvertSuccess(TestSubtask2()),
]
return Sequential(
subtasks=[IncFailed(t) for t in tasks] + [FunctionTask(inc.log)]
)
def on_first_run(self, vision, amlan, blind=False):
initial_tube = amlan.shm_tube.get()
def apply_drop():
# Add tube to table
table_tubes = get_shm_array(shm.recovery_world_table, 'has_tube')
table_tubes[initial_tube] = True
set_shm_array(shm.recovery_world_table, 'has_tube', table_tubes)
self.use_task(
Sequential(
AlignAmlan(
vision,
lambda: vision.obj_with_id(vision.ellipses,
amlan.shm_tube.get()),
amlan,
FastDepth(constants.ellipse_depth),
blind=blind,
align_during_depth=True,
),
Sequential(
Log('Moving to tube drop depth'),
GradualDepth(constants.drop_depth),
) if not blind else NoOp(),
Timer(1),
amlan.Retract(),
FunctionTask(apply_drop),
Sequential(
Log('Moving up away from table slowly'),
GradualDepth(constants.drop_depth + self.AFTER_DROP_DELTA),
) if not blind else NoOp(),
))
def on_first_run(self, vision, *args, **kwargs):
table = shm.recovery_world_table.get()
def record_position():
table.know_pos = True
table.north = shm.kalman.north.get()
table.east = shm.kalman.east.get()
shm.recovery_world_table.set(table)
self.use_task(
Sequential(
# Move to the table depth first so we don't blow things off
Log('Moving to table depth'),
FastDepth(constants.table_depth),
Sequential(
Log('Returning to table position ({}, {})'.format(
table.north, table.east)),
GoToPositionRough(table.north, table.east, optimize=False),
) if table.know_pos else NoOp(),
Log('Searching for table'),
MasterConcurrent(
IdentifyObjects(lambda: vision.ellipses, 2),
SearchWithGlobalTimeout(timeout=self.TIMEOUT)),
Log('Centering table'),
CenterCentroid(lambda: vision.coords(vision.ellipses)),
Zero(),
Log('Recording table position'),
FunctionTask(record_position),
))
def on_first_run(self, task, enable=True, optimize=False):
enable_var = shm.navigation_settings.position_controls
optimize_var = shm.navigation_settings.optimize
init_enable, init_optimize = enable_var.get(), optimize_var.get()
def set_shm(enable, optimize):
enable_var.set(enable)
optimize_var.set(optimize)
self.use_task(
Defer(
Sequential(
FunctionTask(lambda: set_shm(enable, optimize)),
task,
),
FunctionTask(lambda: set_shm(init_enable, init_optimize)),
))
def TrackerSearch():
return \
Retry(
Sequential(
Conditional(FunctionTask(set_second_task_if_possible), on_fail= \
Sequential(
Depth(BOARD_DEPTH, error=0.2),
# PingerTracker goes here
Conditional(SearchBoard(), on_success=FunctionTask(lambda: set_pinger_task(Stake)), on_fail= \
Sequential(
markers.set('center'),
FunctionTask(lambda: set_pinger_task(Recovery))
)
)
)
)
), attempts=3
)
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 SearchCalled():
return Sequential(
Log('Searching for any buoy'),
Zero(),
SearchFor(
While(lambda: VelocityY(DIRECTION * -0.2), True),
triangle_visible,
consistent_frames=(3, 5) #TODO: Check consistent frames
),
FunctionTask(set_last_seen),
Zero())
def on_first_run(self, vision, single_bin, *args, **kwargs):
self.use_task(Conditional(
Sequential(
MoveAboveBins(vision),
Timer(0.5), # Wait for vision to stabilize
FunctionTask(lambda: vision.classify(single_bin)),
),
Log('Bins classified'),
Fail(Log('Failed to classify bins')),
))
def on_first_run(self, vision, tube, amlan, *args, **kwargs):
def apply_grab():
# Add tube to Amlan in shm
amlan.shm_tube.set(tube)
# Remove tube from tower in shm
tower_tubes = get_shm_array(shm.recovery_world_tower, 'has_tube')
tower_tubes[tube] = False
set_shm_array(shm.recovery_world_tower, 'has_tube', tower_tubes)
# Remove tube from vision pattern
vision.remove_tube(tube)
north = shm.kalman.north.get()
east = shm.kalman.east.get()
depth = shm.desires.depth.get()
initial_tubes = sum(t.obs is not None for t in vision.tubes)
self.use_task(
Conditional(
Sequential(
amlan.FastRetract(),
Defer(
Conditional(
GrabTube(vision, tube, amlan),
on_fail=Fail(
Sequential(
Log('Failed to grab, prematurely retracting grabber'
),
amlan.FastRetract(),
)),
),
Sequential(
Zero(),
Log('Restoring position from before grab'),
FastDepth(depth),
GoToPositionRough(north, east, optimize=False),
),
),
# TODO continue always assuming good grab?
# VerifyGrab(vision, initial_tubes),
FunctionTask(apply_grab),
),
Log('Grabbed {} tube with {} Amlan!'.format(
constants.colors[tube].name,
amlan.name,
)),
Fail(amlan.FastRetract()),
))
def DecideAndPush():
counter_wolf = VisibleCounter(shm.bins_status.wolf_visible_frames.get)
counter_bat = VisibleCounter(shm.bins_status.bat_visible_frames.get)
is_bat_getter = lambda: counter_wolf.get_value() < counter_bat.get_value()
counter_wolf_2 = VisibleCounter(shm.bins_status.wolf_visible_frames.get)
counter_bat_2 = VisibleCounter(shm.bins_status.bat_visible_frames.get)
return Sequential(
Depth(0.5),
center_cover(),
DoComparison(counter_wolf, counter_bat),
FunctionTask(lambda: Log('wolf: {}'.format(counter_wolf.get_value()))
()),
FunctionTask(lambda: Log('bat: {}'.format(counter_bat.get_value()))()),
#MasterConcurrent(
Depth(2.5),
# While(center_cover, lambda: True)
#),
LeverWrapper(is_bat_getter=is_bat_getter, wrapped_task=PushLever()),
#Conditional(Timed(FunctionTask(lambda: counter_wolf.get_value() > counter_bat.get_value()), .1),
# on_success=Log("wolf visible"),
# on_fail=Log("bat visible"),
#),
#Log('nani')
Depth(.5),
center_cover(),
DoComparison(counter_wolf_2, counter_bat_2),
FunctionTask(lambda: Log('wolf: {}'.format(counter_wolf_2.get_value()))
()),
FunctionTask(lambda: Log('bat: {}'.format(counter_bat_2.get_value()))
()),
Conditional(
FunctionTask(lambda: decide(is_bat_getter(
), counter_wolf_2.get_value(), counter_bat_2.get_value()),
on_success=full_wolf(),
on_fail=full_bat())))
def TrackerSearch():
global get_pinger_task
global stake
return Retry(lambda: \
Sequential(
Sequential(
Depth(BOARD_DEPTH, error=0.2),
Either(
TrackPinger(),
# Consistent(lambda: get_pinger_task() == stake and shm.torpedoes_stake.board_visible.get(), count=2, total=3, invert=False, result=True)),
Consistent(shm.torpedoes_stake.board_visible.get, count=2, total=3, invert=False, result=True)),
VelocityX(0, error=40),
Log('dafuq'),
Conditional(FunctionTask(set_second_task_if_possible), on_fail= \
Conditional(SearchBoard(), on_success=FunctionTask(lambda: set_pinger_task(stake)), on_fail= \
Sequential(
Log('we cant see jack'),
FunctionTask(lambda: set_pinger_task(surface))
)
)
)
)
), attempts=3
)
def on_first_run(self,
vision,
obj_func,
amlan,
align_depth_task,
blind=False,
align_during_depth=False,
*args,
**kwargs):
def tube_angle_heading():
return shm.kalman.heading.get() + (obj_func().obs.angle % 180 - 90)
self.must_see_obj = not blind
def unsee():
self.must_see_obj = False
self.task = Sequential(
Sequential(
Log('Centering object'),
CenterCentroid(lambda: vision.coords([obj_func()]),
precision=0),
Zero(),
Sequential(
Log('Aligning to object'),
Concurrent(
CenterCentroid(lambda: vision.coords([obj_func()]),
precision=0),
GradualHeading(tube_angle_heading),
finite=False,
),
) if not align_during_depth else NoOp(),
Log('Going down and precisely aligning to object'),
ConsistentTask(
Concurrent(
GradualHeading(tube_angle_heading),
CenterCentroid(lambda: vision.coords([obj_func()]),
precision=2),
align_depth_task,
finite=False,
)),
FunctionTask(unsee),
) if not blind else NoOp(),
PositionalControl(),
Zero(),
Log('Applying Amlan offset'),
amlan.CenterFromDownCam(),
)
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, vision, reset_state=True, *args, **kwargs):
# Limit navigation speed to prevent merge cutting out computer
nav_speed = shm.navigation_settings.max_speed
orig_speed = nav_speed.get()
nav_speed.set(0.5)
if reset_state:
for loc in ['table', 'tower']:
g_name = getattr(shm, 'recovery_world_{}'.format(loc))
g = g_name.get()
g.know_pos = False
for i in range(self.MAX_COLORS):
setattr(g, 'has_tube{}'.format(i), loc == 'tower'
and i < len(constants.colors))
g_name.set(g)
for a in AMLANS:
a.shm_tube.set(-1)
self.use_task(
Conditional(
Defer(
Sequential(
Log('Starting recovery!'),
AMLANS[0].FastRetract(),
AMLANS[1].FastRetract(),
TryRecovery(vision),
),
Sequential(
AMLANS[0].FastRetract(),
AMLANS[1].FastRetract(),
Log('Returning to pre-recovery depth'),
FastDepth(shm.kalman.depth.get()),
Log('Resetting navigation speed'),
FunctionTask(lambda: nav_speed.set(orig_speed)),
Log('Moving away from table'),
Heading(WALL_TOWER_HEADING),
MoveX(5, deadband=0.08),
),
),
Log('Recovery success!'),
Log('Recovery failure!'),
))
def Iteration(first_run=False):
return Sequential(
# Sequential(
# Log('Classifying tubes'),
# Retry(lambda: MoveAboveTower(vision), 3),
# FunctionTask(vision.classify_tubes),
# ) if first_run else NoOp(),
GetTube(vision),
Succeed(GetTube(
vision)), # It's OK if we don't grab the second tube
Surface(),
AvoidWall(),
# Initially classify ellipses as their size / color may change
# as tubes are added
Sequential(
Log('Classifying ellipses'),
Retry(lambda: MoveAboveTable(vision), 3),
FunctionTask(vision.classify_ellipses),
) if first_run else NoOp(),
PlaceTubes(vision),
)
def _RemoveTube(self):
return FunctionTask(lambda: self.shm_tube.set(-1))
# ConfigureHydromath(False, True), # # Go back to normal vision settings # VisionFramePeriod(0.1), # this should be default # ) # DriveToSecondPath = Sequential( # BigDepth(highway_settings.high_depth), # FakeMoveX(dist=highway_settings.dist, speed=highway_settings.speed), # BigDepth(highway_settings.low_depth), # ) BeginMission = MissionTask( name = 'BeginMission', #DON'T CHANGE THIS!!!! cls = Begin, modules = None, surfaces = True ) EndMission = MissionTask( name = 'EndMission', #DON'T CHANGE THIS!!!! cls = End, modules = None, surfaces = True ) ZeroHeading = lambda: FunctionTask(set_zero_heading) # configure_hydrophones = ConfigureHydromath(True, True) # frick = WaitForUnkill(killed=False, wait=1)
if self.angle_1_checker.check(
diff > 0 ^ (angle_1 < angle_2) ^ (not bend_right)):
self.finish()
if self.angle_2_checker.check(
diff < 0 ^ (angle_1 < angle_2) ^ (not bend_right)):
self.finish(success=False)
PipeAlign = lambda heading: Concurrent(
DownwardTarget(lambda: (shm.path_results.center_x.get(),
shm.path_results.center_y.get()),
target=(0, -.25),
deadband=(.1, .1),
px=0.5,
py=0.5), Log("Centered on Pipe!"),
FunctionTask(lambda: shm.navigation_desires.heading.set(
-180 / 3.14 * heading.get() + shm.kalman.heading.get())))
FollowPipe = lambda h1, h2: Sequential(
PipeAlign(h1),
Zero(),
Log("Aligned To Pipe!"),
DownwardTarget(lambda: (shm.path_results.center_x.get(),
shm.path_results.center_y.get()),
target=(0, 0),
deadband=(.1, .1),
px=0.5,
py=0.5),
Zero(),
Log("Centered on Pipe!"),
FunctionTask(lambda: shm.navigation_desires.heading.set(
-180 / 3.14 * h2.get() + shm.kalman.heading.get())),
print('why')
if crucifix_state == 2:
return STATES[0]
return STATES[1]
def crucifix_task():
"""Do crucifix task second. If crucifix not found, do marker 1"""
print('why2')
if crucifix_state == 2:
return STATES[1]
return STATES[0]
Recovery = lambda: Sequential(
Succeed(Timeout(CenterAny(), 20)),
SetMarker('first'),
Conditional(Timeout(SearchCrucifix(), 30), on_success=Succeed(Sequential(CenterCrucifix(), SetMarker('crucifix'), FunctionTask(lambda: change_state(1)))), on_fail=Succeed()),
FunctionTask(reflect),
GoToMarker('second'),
Timeout(SearchAnyVampire(), 40), # TODO: MAKE THIS WORK
Succeed(Timeout(CenterAny(), 40)),
SetMarker('second'),
Conditional(FunctionTask(get_crucifix_found), on_success=Log('Skipping searching crucifix because found already'), on_fail=Conditional(Timeout(SearchCrucifix(), 30), on_success=Sequential(SetMarker('crucifix'), FunctionTask(lambda: change_state(2))), on_fail=Succeed())),
GoToMarker(non_crucifix_task),
# GrabVampire(),
ReleaseVampire(lambda: edge(non_crucifix_task())),
GoToMarker(crucifix_task),
# GrabVampire(),
ReleaseVampire(lambda: edge(crucifix_task())),
Conditional(FunctionTask(get_crucifix_found), on_success=\
Sequential(
GoToMarker('crucifix'),
class FinishIf(Task):
def on_run(self, task, condition, **kwargs):
success = condition()
if success:
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],
def leds_color(port_color, star_color):
leds = shm.leds.get()
port = get_decimal_color(port_color)
star = get_decimal_color(star_color)
leds.port_color_red = port[0]
leds.port_color_green = port[1]
leds.port_color_blue = port[2]
leds.starboard_color_red = star[0]
leds.starboard_color_green = star[1]
leds.starboard_color_blue = star[2]
shm.leds.set(leds)
LightShow = lambda num: FunctionTask(lambda: light_show(num))
Leds = lambda port, star: FunctionTask(lambda: leds_color(port, star))
AllLeds = lambda color: Leds(color, color)
colors = {
'black': '000000',
'red': 'FF0000',
'green': '00FF00',
'blue': '0000FF',
'yellow': 'FFFF00',
'cyan': '00FFFF',
'purple': 'FF00FF',
'white': 'FFFFFF',
'orange': 'FF3000',
'pink': 'FF4545',
return fake_move_y(d)
#return MoveY(d) if os.environ['CUAUV_VEHICLE'] == 'castor' else fake_move_y(d)
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),