)
from mission.missions.actuate import (
FireGreen,
FireRed,
)
from mission.constants.config import cash_in as settings
from mission.missions.will_common import BigDepth
dist1 = 45
dist2 = 52
dist3 = dist1 / 2
stupid_castor = Sequential(
BigDepth(1.5),
Timed(VelocityX(0.4), dist1),
# MoveXRough(20),
VelocityX(0),
Heading(37),
Timed(VelocityX(0.4), dist2),
VelocityX(0),
finite=True)
stupid_castor_2 = Sequential(Heading(10),
Timed(VelocityX(0.4), dist3),
VelocityX(0),
finite=True)
search = SaneHeadingSearch()
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
)),
Log('Driving forward...'),
MasterConcurrent(
Consistent(test=lambda: results_groups.width.get() < settings.gate_width_threshold, count=0.2, total=0.3, invert=True, result=True),
Depth(DEPTH_TARGET),
VelocityX(0.1 if is_mainsub() else 0.1),
While(task_func=lambda: XTarget(x=results_groups.gate_center_x.get, db=0.018), condition=True),
),
# Jank
Timed(VelocityX(0 if is_mainsub() else -0.1), 2),
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())),
Timer(4),
Log("Facing new direction!"),
Zero(),
)
FullPipe = lambda bend_right=False: Sequential(
# Don't do anything stupid
FunctionTask(lambda: shm.path_results.num_lines.set(0)),
BigDepth(settings.depth),
Zero(),
Log("At right depth!"),
Retry(
task_func=lambda: Sequential(
Log("Searching for path..."),
SearchTask(),
Zero(),
Log("Found Pipe!"),
Conditional(
Either(
Sequential(
# Don't lose sight in the first second
Timer(1.0),
# Require a really high fail rate - path vision can be finicky
Consistent(visible_test(1),
from mission.missions.hydrophones import Full as Hydrophones
from mission.missions.roulette import Full as Roulette, DropBall as DropRouletteBall
from mission.missions.cash_in import Full as CashIn
from mission.missions.stupid import *
from mission.constants.region import PATH_1_BEND_RIGHT, PATH_2_BEND_RIGHT, RIGHT_HANDED
from mission.constants.timeout import timeouts
# gate & dead reckon
dist1 = 50
dist2 = 45
GateDeadReckon = lambda: Sequential(
Log('Depthing for gate...'),
BigDepth(2.0),
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'),
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'),
#DownwardTarget(
# cash_in_center,
# target=norm_to_vision_downward(0, 0),
# deadband=norm_to_vision_downward(-0.5, -0.5),
# px=0.0005, py=0.001,
#),
Log('Surfacing at cash-in!'),
BigDepth(0),
)
# --------
gate = MissionTask(
name='Gate',
cls=Gate,
modules=[shm.vision_modules.BicolorGate],
surfaces=False,
timeout=timeouts['gate'],
)
gate_dead_reckon = MissionTask(
name='GateDead',
cls=Sequential(
py=0.5,
depth_bounds=(1, MAX_DEPTH))
else:
return ForwardTarget(
point=lambda: (shm_vars[pick_correct_buoy(num)].center_x.get(),
shm_vars[pick_correct_buoy(num)].center_y.get()),
target=(0, 0.1),
deadband=(db, db),
px=0.08 * mult,
py=0.08 * mult, # y is depth
depth_bounds=(1, MAX_DEPTH))
# 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,
)
Full = lambda: Retry(
lambda: Sequential(
Log('Starting'),
Zero(),
#BigDepth(DEPTH_STANDARD, timeout=8),
# Disabled search for now because if tracker mission sees the table, then we don't need to search
Log('Searching for roulette...'),
Search(),
Zero(),
Log('Centering on roulette...'),
align_roulette_center(db=40, p=0.0005),
Log('Descending on roulette...'),
MasterConcurrent(
BigDepth(DEPTH_TARGET_ALIGN_BIN, timeout=8),
align_roulette_center(db=0, p=0.0002),
),
Log('Aligning with table again...'),
align_roulette_center(db=30, p=0.0002),
Log('Aligning heading with green bin...'),
MasterConcurrent(
align_green_angle(db=5, p=0.5),
align_roulette_center(db=0, p=0.0003),
),
Zero(),
Log('Aligning with table again again...'),
MasterConcurrent(
align_roulette_center(db=50, p=0.0003),
align_green_angle(db=0, p=0.5),
),
Hydrophones(),
),
Zero(),
# This should end up getting run twice because we call it in on_exit... but just in case
TrackerCleanup(),
)
TrackerCleanup = lambda: Sequential(
# Turn off hydromathd
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)