def SwayOnlySearch(speed=0.3, width=2.5, right_first=True):
direction = 1 if right_first else -1
return Sequential(
Log('what'),
Timed(VelocityY(direction*speed), width/(2*speed)),
Timed(VelocityY(-direction*speed), width/(speed)),
Timed(VelocityY(direction*speed), width/(2*speed)),
Zero())
def Spin():
return Sequential(
Timed(VelocityX(1), segment_time), Zero(),
Heading(initial_heading + 90),
Timed(VelocityY(-1), segment_time), Zero(),
Heading(initial_heading + 180),
Timed(VelocityX(-1), segment_time), Zero(),
Heading(initial_heading + 270),
Timed(VelocityY(1), segment_time), Zero(),
Heading(initial_heading),
)
def firstrun(self):
self.start = time.time()
self.surge = VelocityX(.2)
self.sway = VelocityY(.2)
self.surge()
self.sway()
def on_first_run(self, *args, **kwargs):
# x-axis on the camera corresponds to sway axis for the sub
self.pid_loop_x = PIDLoop(output_function=VelocityY(), negate=True)
self.pid_loop_y = PIDLoop(output_function=VelocityX(), negate=False)
self.px_default = 0.4
self.py_default = 0.8
PositionalControl(False)()
def testLeftRestore():
return TrackMovementY(
Sequential(
Timed(VelocityY(-.4), 6),
Log('Restoring?'),
RestorePosY(.2),
Zero(),
))
def on_first_run(self, depth_bounds=(None, None), *args, **kwargs):
self.pid_loop_x = PIDLoop(output_function=VelocityY(), negate=True)
self.pid_loop_y = PIDLoop(output_function=RelativeToCurrentDepth(
min_target=depth_bounds[0], max_target=depth_bounds[1]),
negate=True)
self.px_default = 0.8
self.py_default = 0.8
PositionalControl(False)()
def on_run(self, pitch=True, roll=True):
Depth(shm.kalman.depth.get())()
PositionN(shm.kalman.north.get(), positional_controls=None)()
PositionE(shm.kalman.east.get(), positional_controls=None)()
Pitch(0)() if pitch else Pitch(shm.kalman.pitch.get())()
Roll(0)() if roll else Roll(shm.kalman.roll.get())()
VelocityX(0, positional_controls=None)()
VelocityY(0, positional_controls=None)()
self.finish()
def run(self):
print("Centering on pipe")
self.center()
if self.center.finished or self.this_run - self.start > 12:
VelocityX(0)()
VelocityY(0)()
self.center._finish()
self._finish()
def on_first_run(self, vision, *args, **kwargs):
self.pid = PIDLoop(
input_value=lambda: bbar_angle(vision),
output_function=VelocityY(),
target=0,
deadband=3,
p=0.8,
d=0.4,
)
self.depth = Depth(constants.WIRE_DEPTH)
def enter_follow(self):
self.logi("Following a heading of %0.3f" % self.heading_to_pinger)
self.follow_change_heading = Heading()
self.follow_inital_heading = Heading(self.heading_to_pinger + heading_offset)
self.follow_vel_x = VelocityX()
self.follow_vel_y = VelocityY()
distance_to_pinger = self.elevation_to_distance(self.follow_elevation)
self.follow_vel_x(x_dir * self.get_follow_speed(distance_to_pinger))
self.follow_vel_y(0.0)
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 __init__(self, timeout, speed=.3, compensate=False, *args, **kwargs):
super().__init__(*args, **kwargs)
# self.logv("Starting {} task".format(self.__class__.__name__))
self.speed = speed
self.ram_task = VelocityX()
self.strafe_task = VelocityY()
self.commit_task = Sequential(VelocityX(1), Timer(1), VelocityX(0))
self.ram_commit_phase = False
self.TIMEOUT = timeout
self.compensate = compensate
def firstrun(self, timeout=45):
self.surge_time = time.time()
self.sway_time = time.time() - 3
self.start = time.time()
self.surge = VelocityX()
self.sway = VelocityY()
self.sway_speed = .4
self.swaying = True
self.surging = False
self.begin_search = time.time()
self.sway(self.sway_speed)
print("IN PIPE")
def __init__(self, validator, right=True, timeout=10, *args, **kwargs):
"""
validator - a function that returns True when a buoy is found and False
otherwise.
right - determines whether the submarine should move right or
left during its search
timeout - the amount of time to surge
"""
super().__init__(*args, **kwargs)
# self.logv("Starting {} task".format(self.__class__.__name__))
self.validator = validator
self.right = right
self.surge_task = VelocityY()
self.zero_task = Zero()
self.depth_task = Depth(constants.BUOY_SEARCH_DEPTH)
self.TIMEOUT = timeout
def on_first_run(self, clockwise=True, steps=5, spins=2, *args, **kwargs):
delta_heading = [-1, 1][clockwise] * 45
subspins = [MasterConcurrent(CheckSpinCompletion(2),
RelativeToCurrentHeading(delta_heading))]
self.use_task(Sequential(
Timed(VelocityX(1), 1), Zero(),
MasterConcurrent(
Sequential(subtasks=subspins),
VelocityX(lambda: 2 * math.cos(math.radians(shm.kalman.heading.get()))),
VelocityY(lambda: 10 * math.sin(math.radians(shm.kalman.heading.get()))),
),
Zero(),
))
def make_repeat(self, forward, stride, rightFirst, checkBehind):
dir = 1 if rightFirst else -1
self.back_check = Timed(VelocityX(-.3), forward)
self.checked_b = not checkBehind
self.repeat = Sequential(
Timed(VelocityX(.3), forward),
VelocityX(0.0),
Timed(VelocityY(.3 * dir), stride),
VelocityY(0.0),
Timed(VelocityY(-.3 * dir), stride * 2),
VelocityY(0.0),
Timed(VelocityY(.3 * dir), stride),
VelocityY(0.0),
)
def on_first_run(self):
self.has_made_progress = True
self.seen_frames_checker = ConsistencyCheck(3, 3, strict=False)
def location_validator(buoy):
return self.seen_frames_checker.check(buoy.probability.get() != 0)
self.depth_task = DepthRestore()
self.heading_task = HeadingRestore()
self.up_task = DepthRestore(constants.BUOY_OVER_DEPTH)
self.dodge_vel = -.4 if yellowRight else .4
self.over_task = Sequential(Timed(VelocityY(self.dodge_vel), 2),
DirectionalSurge(6, .4, compensate=True))
self.heading_task = HeadingRestore()
self.task = Sequential(
Depth(constants.BUOY_SEARCH_DEPTH),
self.heading_task,
# Depth(0.9, error=.01)
# SeqLog("Looking for red buoy"), LocateFirstBuoy(lambda: location_validator(red_buoy_results), forward=True, right=BUOY_RIGHT_TO_REACH[0], middle=yellow_buoy_results),
Buoy(red_buoy_results, first_buoy=True, right=not redRight()),
# self.depth_task,
# SeqLog("Looking for green buoy stage 1"), LocateBuoyStrafe(lambda: location_validator(yellow_buoy_results), right=True, timeout=3),
SeqLog("Looking for green buoy"),
LocateBuoyStrafe(lambda: location_validator(green_buoy_results),
right=not greenRight(),
timeout=3),
Buoy(green_buoy_results, right=not greenRight()),
# self.depth_task,
SeqLog("Looking for yellow buoy"),
LocateBuoyStrafe(lambda: location_validator(yellow_buoy_results),
right=not yellowRight(),
timeout=2),
Buoy(yellow_buoy_results, right=not yellowRight(), yellow=True),
HeadingRestore(),
SeqLog("Rising to Over depth"),
self.up_task,
SeqLog("Going around buoys"),
self.over_task)
Sequential(
Timer(15),
_Grab()),
RelativeToCurrentDepth(DESCEND_DEPTH, error=0.2),
),
RelativeToInitialDepth(-LID_DEPTH_1, error=0.25),
Log('what'),
Conditional(Yike(), on_fail=Fail(_Release())),
GrabVampireOpenCoffin()
)
Yike = lambda: \
Sequential(
MasterConcurrent(
Sequential(Timed(RelativeToCurrentDepth(-LID_DEPTH), 3.5), RelativeToCurrentDepth(0)),
VelocityY(0.2 * direction_closed())
),
Timed(VelocityY(0.3), 3),
Depth(SEARCH_DEPTH, error=0.2),
GoToMarker('before_grab'),
Timeout(Consistent(visible_open, count=1.5, total=2.0, invert=False, result=True), 10),
Log('Opened Coffin Successfully'),
UnsetMarker('before_grab'),
)
grabbing = False
def get_grabbing():
global grabbing
return grabbing
def on_first_run(self, vel, *args, **kwargs):
if shm.jank_pos.y.get() < 0:
self.task = VelocityY(vel)
else:
self.task = VelocityY(vel * -1)
params = [(step, timeout, pause, deadband) for step in depth_steps]
#self.use_task(Sequential(*interleave(tasks_from_params(Depth, depth_steps), tasks_from_param(Timer, timeout, length=steps))))
self.use_task(
Sequential(*tasks_from_params(Descend, params, tup=True)))
FakeMoveX = lambda dist, speed: Sequential(
MasterConcurrent(Timer(abs(dist / speed)),
VelocityX(-speed if dist < 0 else speed)),
MasterConcurrent(Timer(0.2), VelocityX(speed if dist < 0 else -speed)),
MasterConcurrent(Timer(0.2), VelocityX(0)),
)
FakeMoveY = lambda dist, speed: Sequential(
MasterConcurrent(Timer(abs(dist / speed)),
VelocityY(-speed if dist < 0 else speed)),
MasterConcurrent(Timer(0.2), VelocityY(speed if dist < 0 else -speed)),
MasterConcurrent(Timer(0.2), VelocityY(0)),
)
# A version of VelocitySwaySearch, but better.
# We use this for minisub because it doesn't drift as much.
class ForwardSearch(Task):
def make_repeat(self, forward, stride, speed, rightFirst):
dir = 1 if rightFirst else -1
self.repeat = Sequential(
FakeMoveY(stride * speed, speed * dir),
FakeMoveX(forward * speed, speed),
FakeMoveY(stride * speed, -speed * dir),
FakeMoveY(stride * speed, -speed * dir),
import shm
results_groups = shm.bicolor_gate_vision
class Consistent(Task):
def on_first_run(self, test, count, total, invert, result):
# Multiple by 60 to specify in seconds
self.checker = ConsistencyCheck(count * 60, total * 60, default=False)
def on_run(self, test, count, total, invert, result):
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
)
align_on_three_elem = lambda:\
Sequential(
Log('aligning on three elems'),
ConsistentTask(
# while we CAN see all gates
FinishIf(
task=Concurrent(
# align leftmost and rightmost by length
PIDLoop(
input_value=lambda: shm.gate.rightmost_len.get() / shm.gate.leftmost_len.get(),
target=1,
p=0.5,
deadband=0.1,
output_function=VelocityY()
),
# align to the center of the leftmost and rightmost
PIDLoop(
input_value=lambda: (shm.gate.leftmost_x.get() + shm.gate.rightmost_x.get()) / 2,
target=lambda: shm.gate.img_width.get() / 2,
p=0.2,
deadband=alignment_tolerance_fraction,
output_function=RelativeToCurrentHeading(),
negate=True
),
finite=False
),
condition=lambda: gate_elems() < 3 or is_aligned()
)
),
def on_run(self, *args, **kwargs):
self.align()
if self.align.finished and not self.align.success:
VelocityX(0)()
VelocityY(0)()
# approach_left = ApproachAndTargetFunnel(shm.recovery_vision_forward_red)
# approach_right = ApproachAndTargetFunnel(shm.recovery_vision_forward_red)
drop_left = DropInFunnel(shm.recovery_vision_forward_red, is_left=True)
drop_right = DropInFunnel(shm.recovery_vision_forward_red, is_left=False)
search_left = SearchBin(make_bin_chooser(True))
search_right = SearchBin(make_bin_chooser(False))
pickup_left = PickupFromBin(make_bin_chooser(True), is_left=True)
pickup_right = PickupFromBin(make_bin_chooser(False), is_left=False)
pickup_all = Sequential(
pickup_left,
Log("Picked Up Left"),
Timed(VelocityY(0.2), 2),
VelocityY(0),
pickup_right,
Log("Picked Up Right"),
)
drop_all = Sequential(
drop_right,
Log("Dropped Right"),
drop_left,
Log("Dropped Left"),
)
do_it_all = Sequential(
search_left,
Log("Found"),
def stop():
return Concurrent(
VelocityX(0),
VelocityY(0),
)
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()))
def stop(self):
VelocityY(0)()
RelativeToCurrentDepth(0)()
def make_repeat(self, forward, stride, speed, rightFirst, checkBehind):
dir = 1 if rightFirst else -1
if checkBehind:
self.repeat = Sequential(Timed(VelocityX(-speed), forward),
Timed(VelocityX(speed), forward),
VelocityX(0),
Timed(VelocityY(speed * dir), stride),
VelocityY(0.0),
Timed(VelocityX(speed), forward),
VelocityX(0.0),
Timed(VelocityY(-speed * dir), stride),
VelocityY(0.0),
Timed(VelocityY(-speed * dir), stride),
VelocityY(0.0),
Timed(VelocityX(speed), forward),
VelocityX(0.0),
Timed(VelocityY(speed * dir), stride),
VelocityY(0.0))
else:
self.repeat = Sequential(Timed(VelocityY(speed * dir), stride),
VelocityY(0.0),
Timed(VelocityX(speed), forward),
VelocityX(0.0),
Timed(VelocityY(-speed * dir), stride),
VelocityY(0.0),
Timed(VelocityY(-speed * dir), stride),
VelocityY(0.0),
Timed(VelocityX(speed), forward),
VelocityX(0.0),
Timed(VelocityY(speed * dir), stride),
VelocityY(0.0))
def stop(self):
VelocityY(0)()
VelocityX(0)()
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),
)
)
)
)
