def on_first_run(self, is_bat_getter, wrapped_task):
is_bat = is_bat_getter()
get_x = shm.bins_status.bat_x.get if is_bat else shm.bins_status.wolf_x.get
get_y = shm.bins_status.bat_y.get if is_bat else shm.bins_status.wolf_y.get
pointing_wrong_way = -1 if is_bat ^ (get_x() < 0) else 1
x_amt = .8
y_amt = 1.5 if is_bat else -1.5
turn_amt = -90 if is_bat else 90
r_x_amt = x_amt * pointing_wrong_way
r_y_amt = y_amt * pointing_wrong_way
r_turn_amt = turn_amt * pointing_wrong_way
ret_y_amt = -.8 if is_bat else .8
self.use_task(
Sequential(
FakeMoveY(r_y_amt, .3),
Timer(.2),
FakeMoveX(r_x_amt, .3),
Timer(.2),
RelativeToInitialHeading(r_turn_amt),
wrapped_task,
Log("Moving back"),
# might have to deal with heaeding here
FakeMoveX(-.8, .3),
Log("moving Y {}".format(ret_y_amt)),
FakeMoveY(ret_y_amt, .3), # intentionally swapped
Timer(.5),
#Log("moving X {}".format(y_amt * math.sin(math.radians(turn_amt)))),
#FakeMoveX(y_amt * math.sin(math.radians(turn_amt)), .3), # intentionally swapped
FakeMoveX(1.5, .3),
))
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, vision, *args, **kwargs):
initial_heading = shm.kalman.heading.get()
depth_set = DepthRestore()
self.use_task(
Conditional(
Sequential(
MasterConcurrent(
Sequential(
Retry(lambda: Sequential(
Log('Returning to initial heading'),
Heading(initial_heading),
Log('Going to depth'),
depth_set,
#Log('Moving forward away last pos'),
#Timed(VelocityX(0.5), 1),
#Zero(),
Log('Searching for gate'),
MasterConcurrent(
IdentifyGate(vision),
VelocityHeadingSearch(initial_heading=initial_heading),
),
Zero(),
Log('Found gate, aligning'),
AlignChannel(vision),
), float('inf')),
),
Fail(Timer(180)),
),
Log('Aligned to gate, moving closer and fixing depth'),
MoveCloser(2),
Log('Beginning spin'),
StyleSegmentedSpin(),
),
Sequential(
Log('Wire completed successfully!'),
Timed(VelocityX(.4), 2),
Zero(),
RelativeToInitialHeading(180),
),
Log('Traveled too far without task completion'),
)
)
def on_first_run(self, angle, dps=45):
self.degrees_per_second = dps
self.revolve_angle = 0
self.revolve_task = RelativeToInitialHeading(
current=shm.kalman.heading.get())
Pitch(angle)()
from mission.framework.combinators import Sequential, Retry, While
from mission.framework.movement import RelativeToInitialHeading, VelocityY, VelocityX
from mission.framework.position import MoveY
from mission.framework.primitive import Log, Fail, Succeed, FunctionTask
from mission.framework.timing import Timed
from mission.missions.will_common import FakeMoveY
sides = 6
def loop_state():
current = -1
def iterate():
nonlocal current
current += 1
return current < sides
return iterate
polygon = Sequential(
While(
lambda: Sequential(Timed(VelocityY(-0.2), 6),
RelativeToInitialHeading(360 / sides),
Timed(VelocityY(-0.2), 6)), loop_state()))
from mission.framework.combinators import Sequential, Concurrent
from mission.framework.movement import RelativeToInitialHeading
from mission.framework.position import MoveX
from mission.framework.primitive import Log
sides = 8
tasks = list(map(
lambda i: Sequential(Log("Beginning side {}".format(i)), MoveX(4 / sides), RelativeToInitialHeading(360 / sides)),
range(sides)
))
print(tasks)
polygon = Sequential(subtasks=tasks)
def InterMoveY(d):
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),
def on_first_run(self, shm_group, is_left, *args, **kwargs):
APPROACH_DIST = settings.drop_approach_dist
DVL_FORWARD_CORRECT_DIST = settings.drop_dvl_forward_correct_dist[is_left]
DVL_ANGLE_CORRECT = settings.drop_heading_correct[is_left]
APPROACH_FUNNEL_DEPTH = settings.approach_funnel_depth
turn_task = RelativeToInitialHeading(90 if is_left else -90)
reset_heading_task = RelativeToInitialHeading(0)
reset_heading_task()
# reset_pos_task = MoveXYRough((0, 0))
# reset_pos_task()
self.reset_pos_target = None
drop_task = FireRed if is_left else FireGreen
def record_pos():
self.reset_pos_target = (shm.kalman.north.get(), shm.kalman.east.get())
Y_DIST = -APPROACH_DIST if is_left else APPROACH_DIST
self.use_task(
Sequential(
VisionSelector(forward=True),
WithPositionalControl(
cons(Depth(APPROACH_FUNNEL_DEPTH))
),
ApproachAndTargetFunnel(shm_group),
stop(),
FunctionTask(record_pos),
VisionSelector(downward=True),
WithPositionalControl(
Sequential(
Log("Aligned"),
Log("Turning..."),
cons(turn_task),
Log("Surfacing..."),
cons(Depth(-.05)),
Log("Moving..."),
cons(
Concurrent(
MoveXYRough((DVL_FORWARD_CORRECT_DIST, Y_DIST)),
RelativeToInitialHeading(DVL_ANGLE_CORRECT),
finite=False
),
debug=True),
WithPositionalControl(
MasterConcurrent(
cons(
FunctionTask(
lambda: shm.recovery_vision_downward_red.probability.get() > .5,
finite=False
),
total=30,
success=10,
debug=True
),
VelocityY(.1 * (-1 if is_left else 1)),
),
enable=False
),
stop(),
Log("Over, dropping!..."),
drop_task(),
# Timer(3),
Log("Moving back..."),
WithPositionalControl(
Sequential(
Timed(VelocityY(-.1 * (-1 if is_left else 1)), 2),
stop(),
),
enable=False
),
cons(GoToPositionRough(lambda: self.reset_pos_target[0], lambda: self.reset_pos_target[1]), debug=True),
Log("Diving..."),
cons(Depth(.5)),
Log("Turning back..."),
cons(reset_heading_task, debug=True),
)
)
)
)
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,
]
CenterCentroid,
Disjunction,
ConsistentTask,
PrintDone,
Altitude,
AlignAmlan,
# AMLANS,
Infinite,
Except,
)
from mission.framework.search import (
SearchFor,
VelocitySwaySearch,
)
from mission.missions.actuate import (
FireGreen,
FireRed,
)
from mission.constants.config import cash_in as settings
stupid_castor = Sequential(
# Timed(VelocityX(0.4), 30),
MoveXRough(50),
VelocityX(0),
RelativeToInitialHeading(50),
Timed(VelocityX(0.4), 60),
finite=True
)
name='GateDead',
cls=Sequential(
Zero(),
Log('Dead reckoning through gate...'),
BigDepth(2.2),
FakeMoveX(dist=5.8, speed=0.2),
),
modules=None,
surfaces=False,
)
fake_path = lambda bend_right: lambda: MissionTask(
name='FakePath',
cls=Sequential(
FakeMoveX(dist=0.5, speed=0.2),
RelativeToInitialHeading(45 if bend_right else -45),
),
modules=None,
surfaces=False,
)
get_path = lambda bend_right: lambda: MissionTask(
name='Path',
cls=PathGetter(bend_right),
modules=[shm.vision_modules.Pipes],
surfaces=False,
timeout=timeouts['path'],
on_timeout=RelativeToInitialHeading(45 if bend_right else -45),
)
dice = MissionTask(
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),
FakeMoveY(.8, .65), Timed(VelocityY(-.5), .05), Zero(),
Timer(1), Timed(Concurrent(VelocityX(.5), FollowLine), 25),
FunctionTask(lambda: shm.switches.soft_kill.set(1)))
#bbb = Sequential(
# MoveY(.375, deadband=.05),
# Timed(Concurrent(VelocityX(.5), FollowLine), 23),
#)
PipeAlign = lambda heading, trg, dst, deadband: Sequential(
Log("PipeAlign start"),
# lambda: shm.vamp_buoy_results.jiangshi_visible.get() and single_buoy_size() > SEARCH_SIZE_THRESH,
consistent_frames=(3, 5)),
Log('Singular Found'),
Zero())
DeadReckonStupid = lambda: \
Sequential(
# Timed(VelocityY(DIRECTION * 0.3, error=40), 3),
# VelocityY(0, error=40),
# Timed(VelocityX(0.3, error=40), 15),
# VelocityX(0, error=40),
# SlowHeading(),
Log('Backing up'),
Timed(VelocityX(-0.2), 6),
Log('finding buoy'),
RelativeToInitialHeading(DIRECTION * 90)
)
heading = None
def store_heading(h):
global heading
print('storing heading %s' % h())
heading = h()
def get_heading():
global heading
if heading == None:
heading = shm.kalman.heading.get()
# If this is the second buoy, then don't search again goddamit
on_fail=NoOp(),
))
# This is the radius of the dots on the die
MIN_DOT_RADIUS = settings.min_dot_radius
BackUp = lambda: Sequential(
Log('Backing up...'),
# Should be rammed, back up until we can see both buoys
# If we see the second buoy then we can see both
BackUpUntilVisible(
num=1, speed=0.08, timeout=settings.rammed_back_up_timeout),
)
reset_heading = RelativeToInitialHeading(0)
RamBuoyAttempt = lambda num: Sequential(
Log('Ramming buoy {}'.format(num)),
Succeed(reset_heading),
Conditional(
Either(
Consistent(lambda: shm_vars[pick_correct_buoy(num)].visible.get(),
count=0.5,
total=1.5,
invert=True,
result=False),
Sequential(
MasterConcurrent(
Consistent(lambda: shm_vars[pick_correct_buoy(num)].
radius_norm.get() < MIN_DOT_RADIUS,