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 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, vision, *args, **kwargs):
check = lambda: checkAligned(vision)
self.use_task(
Conditional(
While(
lambda: Sequential(
Log("Restoring depth"),
Depth(constants.WIRE_DEPTH),
Log('Align Heading'),
AlignHeading(vision),
Zero(),
Log('Align Sway'),
AlignSway(vision),
Zero(),
Log('Align Heading'),
AlignHeading(vision),
Zero(),
Log('Align Fore'),
AlignFore(vision),
Zero(),
), lambda: checkNotAligned(vision)
),
on_fail=Sequential(
Log('Lost the gate, backing up and looking again'),
Succeed(Concurrent(
Fail(Timed(VelocityX(-.4), 6)),
Fail(IdentifyGate(vision)),
)),
Fail(),
)
)
)
def one_pipe(grp):
return Timeout(
45,
Sequential(
Log('Going to Search Depth'), Depth(PIPE_SEARCH_DEPTH), Zero(),
Log('Sway searching for pipe with Behind'),
TrackMovementY(search_task_behind()),
Retry(
lambda: Sequential(
Zero(),
Log('Sway searching for pipe; may have been lost'),
TrackMovementY(search_task(), shm.jank_pos.y.get()),
Log('Centering on pipe'),
Conditional(
center(),
on_fail=
Fail(
Sequential(
Log("Pipe lost, Attempting to Restore Y pos"),
Zero(),
TrackMovementY(RestorePosY(.3),
shm.jank_pos.y.get()),
))),
Depth(PIPE_FOLLOW_DEPTH),
Log('Aligning with pipe'),
Concurrent(align(), center(), finite=False),
), float("inf")), Zero(), Log('Aligned, moving forward'),
Timed(VelocityX(.4), 3),
Zero()
#Depth(PIPE_FOLLOW_DEPTH)
))
def on_first_run(self, *args, **kwargs):
init_heading = shm.kalman.heading.get()
self.use_task(Timed(
While(lambda: Sequential(
# Pause a little to let object-recognizing tasks see the current fov
Timer(0.5),
# Check the right side
GradualHeading(init_heading + 90),
Timer(0.5),
Heading(init_heading),
# Check the left and back
GradualHeading(init_heading - 90),
GradualHeading(init_heading - 180),
GradualHeading(init_heading - 270),
Timer(0.5),
Heading(init_heading),
# Move back a bit, we might be too close
MoveX(-1),
), True),
self.TIMEOUT,
))
def both_paths():
return Sequential(Log('Going to Search Depth'), Depth(PATH_SEARCH_DEPTH),
Zero(), Log('Beginning to align to Path 1'),
one_path(shm.path_results_1), Log('Moving forward'),
Timed(VelocityX(.2), 1),
Log('Beginning to align to Path 2'),
one_path(shm.path_results_2), Log('Done'), Zero())
def second_pipe(grp):
return Sequential(
Log('Going to Search Depth'),
Depth(PIPE_SEARCH_DEPTH),
Retry(lambda: Sequential(
Zero(),
Log('Sway searching for pipe'),
search_task(),
Log('Centering on pipe'),
center(),
Depth(PIPE_FOLLOW_DEPTH),
Log('Aligning with pipe'),
Concurrent(
align(),
center(),
finite=False
),
), float("inf")),
Zero(),
Log('Aligned, moving forward'),
Timed(VelocityX(.4),3),
Zero()
#Depth(PIPE_FOLLOW_DEPTH)
)
def on_first_run(self, time, *args, **kwargs):
self.use_task(Sequential(
Depth(constants.WIRE_THRU_DEPTH, error=.1),
Timed(VelocityX(.3), time),
Zero(),
#Timed(VelocityY(.2), 1),
Zero()
))
def testLeftRestore():
return TrackMovementY(
Sequential(
Timed(VelocityY(-.4), 6),
Log('Restoring?'),
RestorePosY(.2),
Zero(),
))
def on_first_run(self, *args, **kwargs):
self.use_task(
Sequential(
Log('Going to gate depth'),
Depth(constants.depth),
Log('Moving through gate'),
Timed(VelocityX(0.4), constants.vel_time),
Zero(),
))
def DoComparison(counter_a, counter_b):
return \
While(
lambda: Timed(Concurrent(
While(center_cover, lambda: True),
counter_a,
counter_b,
), 1.5),
lambda: counter_a.get_value() + counter_b.get_value() < 10
)
def on_first_run(self, vision, *args, **kwargs):
self.pid = PIDLoop(
input_value=lambda: bbar_width_ratio(vision),
output_function=VelocityX(),
target= constants.BBAR_RATIO_TARGET,
deadband=constants.DEFAULT_DEADBAND,
p=1,
d=4
)
self.retreat_task = Sequential(Log('Retreat'), Timed(VelocityX(-.3), 1))
def on_first_run(self, vision, *args, **kwargs):
self.use_task(Zeroed(Timeout(
Sequential(
Log('Attempting to grab handle'),
GetHandle(vision, AMLANS[0]),
Log('Carrying cover away'),
RelativeToInitialDepth(-self.PICKUP_DELTA_DEPTH, error=0.1),
Timed(VelocityX(self.SLIDE_SPEED), self.SLIDE_TIME),
Zero(),
Log('Dropping cover off here'),
AMLANS[0].Retract(),
Timer(1.5),
Log('Attempting to return to near pre-grab location'),
RelativeToInitialDepth(-1),
Timed(VelocityX(-self.SLIDE_SPEED), self.SLIDE_TIME * 5 / 6),
Zero(),
), self.TOTAL_TIMEOUT)))
def Full():
return Sequential(
Log('Doing balls! RIP octagon'),
Log('Turning away from wall'),
Heading(WALL_TOWER_HEADING, error=5),
Log('Surfacing'),
Depth(0.5, error=0.1),
Timed(Depth(0), 3),
Log('Going over to press octagon'),
Timed(VelocityX(0.3), 11),
Log('Pulling octagon underwater'),
Timed(Concurrent(
VelocityX(0.3),
Depth(0.5),
finite=False,
), 5),
Log('Snapping octagon back in place'),
Timed(VelocityX(-0.2), 4),
VelocityX(0),
Log('Well, I guess that\'s it for balls.'),
)
def on_first_run(self, *args, **kwargs):
original_depth = shm.kalman.depth.get()
self.use_task(
WithPositionalControl(
Sequential(
Log('Getting close to surface'),
FastDepth(0.5),
Log('Surfacing!'),
Timed(Depth(0), 3),
Log('Falling back below surface'),
FastDepth(original_depth),
)))
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, 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 on_first_run(self, vision, amlan, *args, **kwargs):
self.use_task(Zeroed(Sequential(
Log('Aligning Amlan'),
AlignAmlan(
vision,
vision.target_bin,
amlan,
Depth(constants.above_bin_depth),
),
Log('Moving to handle grab depth'),
Timed(Depth(constants.grab_depth), self.GRAB_TIME),
Log('Extending Amlan'),
amlan.Extend(),
Timer(2),
)))
def on_first_run(self,
initial_heading=None,
stride_speed=1,
stride_time=1.5,
heading_amplitude=45,
*args,
**kwargs):
if initial_heading is None:
initial_heading = shm.desires.heading.get()
Heading(initial_heading)(),
self.use_task(
While(
lambda: Sequential(
Timed(VelocityX(stride_speed), stride_time),
Zero(),
HeadingSearch.Scan(initial_heading + heading_amplitude),
HeadingSearch.Scan(initial_heading - heading_amplitude),
Heading(initial_heading),
), True))
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)
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 = Timed(VelocityX(.4), 8)
self.heading_task = HeadingRestore()
self.task = Sequential(
Depth(constants.BUOY_SEARCH_DEPTH),
Conditional(
MasterConcurrent(
Sequential(
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=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=greenRight(),
timeout=3),
Buoy(green_buoy_results, right=greenRight()),
# self.depth_task,
SeqLog("Looking for yellow buoy"),
LocateBuoyStrafe(
lambda: location_validator(yellow_buoy_results),
right=yellowRight(),
timeout=2),
Buoy(yellow_buoy_results,
right=yellowRight(),
yellow=True),
Log("re-aligning red buoy"),
LocateBuoyStrafe(
lambda: location_validator(red_buoy_results),
right=secondRedRight(),
timeout=2),
Buoy(red_buoy_results,
right=secondRedRight(),
yellow=False,
align_only=True),
),
PreventSurfacing(),
),
on_success=Sequential(
Zero(),
self.heading_task,
SeqLog("Rising to Over depth"),
self.up_task,
SeqLog("Going over buoys"),
self.over_task,
),
on_fail=Sequential(
Zero(),
self.heading_task,
SeqLog("Going to Over depth"),
self.up_task,
SeqLog("Going over buoys"),
self.over_task,
Timed(VelocityX(.4), 8),
)))
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),
)
)
)
)
)
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()
# Require a really high fail rate - path vision can be finicky
Consistent(visible_test(1),
count=2.5,
total=3,
result=False,
invert=True),
),
Conditional(
FirstPipeGroupFirst(bend_right),
on_success=FollowPipe(shm.path_results.angle_1, shm.
path_results.angle_2),
on_fail=FollowPipe(shm.path_results.angle_2, shm.
path_results.angle_1)),
),
on_success=Sequential(
Timed(VelocityX(.1), settings.post_dist),
Log("Done!"),
Zero(),
Log("Finished path!"),
),
on_fail=Fail(
Sequential(
Log("Lost sight of path. Backing up..."),
FakeMoveX(-settings.failure_back_up_dist,
speed=settings.failure_back_up_speed),
), ),
),
),
attempts=5))
path = FullPipe()
def make_move(self):
t = (self.loop + (self.side == 3)) * 6
print("Moving for {}".format(t))
return Timed(VelocityX(0.3), t)
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))
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()))
# 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),
VelocityX(0),
Log('Lining up with red side...'),
ConsistentTask(Concurrent(
Depth(DEPTH_TARGET),
XTarget(x=results_groups.gate_center_x.get, db=0.05),
finite=False,
)),
Log('Charging...'),
Timed(VelocityX(0.5 if is_mainsub() else 0.2), settings.charge_dist),
Log('Through gate!'),
)
)
align_task = lambda:\
Sequential(
While(
condition=lambda: not is_aligned(),
task_func=lambda: Sequential(
Conditional(
main_task=FunctionTask(lambda: gate_elems() == 2),
on_success=align_on_two_elem(),
on_fail=Conditional(
main_task=FunctionTask(lambda: gate_elems() == 3),
on_success=align_on_three_elem(),
on_fail=Sequential(
Log('we see less than two elems, failed'),
Timed(VelocityX(-0.3), 2),
),
finite=False
),
finite=False
),
Zero(),
#Timer(1),
finite=False
),
),
finite=False,
)
align_left_width = lambda: PIDLoop(
input_value=lambda: shm.gate.middle_x.get() - shm.gate.leftmost_x.get(),
