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 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,
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 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 PushLever():
def TargetLever(py):
return ForwardTarget(
point=lambda:
(shm.bins_status.lever_x.get(), shm.bins_status.lever_y.get() /
(1 + shm.bins_status.lever_sz.get() / 50)),
#target=lambda: (0, .3 * min(100, shm.bins_status.lever_sz.get()) / 100),
target=lambda: (0, .25 /
(1 + shm.bins_status.lever_sz.get() / 50)),
valid=shm.bins_status.lever_visible.get,
deadband=(DEADBAND, DEADBAND),
px=1,
py=py,
ix=.05,
iy=.05)
return Sequential(
Log("PushLever start"),
FunctionTask(VelocityX(.2)),
MasterConcurrent(
Consistent(lambda: shm.bins_status.lever_sz.get() > 90,
count=.5,
total=.75,
invert=False,
result=True),
While(lambda: TargetLever(1.5), lambda: True),
While(
lambda: FunctionTask(
VelocityX(.2 / (1 + 2 * (abs(shm.bins_status.lever_x.get(
)) + abs(shm.bins_status.lever_y.get() - .25))))),
lambda: True)),
#Log("Higher P"),
#MasterConcurrent(
# Consistent(lambda: shm.bins_status.lever_sz.get() > 100, count=.5, total=.75, invert=False, result=True),
# While(lambda: TargetLever(.8), lambda: True),
# While(lambda: FunctionTask(
# VelocityX(.2 / (1 + 2 * (abs(shm.bins_status.lever_x.get()) + abs(shm.bins_status.lever_y.get()-.15))))
# ), lambda: True)
#),
#Log("targeting"),
#TargetLever(),
Log("zoom zoom"),
#RelativeToInitialDepth(-.1),
#Timed(RelativeToCurrentDepth(-2), .7),
#FunctionTask(RelativeToInitialDepth(0)),
Concurrent(
Sequential(
Timer(1),
Log("Forwards!"),
FunctionTask(VelocityX(1)),
#VelocityDepth(0),
),
Sequential(
Timed(RelativeToCurrentDepth(-.4), 1),
RelativeToInitialDepth(0),
),
Timer(4),
),
#Timed(
# While(TargetLever, lambda: True),
# 5
#),
Timed(VelocityX(-.8), .5),
VelocityX(0),
Log("waiting"),
Timer(4),
#RelativeToInitialHeading(0),
#Timed(VelocityX(-.8), 1),
#RelativeToInitialHeading(0),
#FunctionTask(VelocityX(0)),
#Timer(5),
#TargetLever()
)
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
)
# 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()
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()))
)
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(),
MasterConcurrent(
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
# 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!'),
)
# 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 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 timed(task, timeout=2):
return Timed(task, timeout)
def make_move(self):
t = (self.loop + (self.side == 3)) * 6
print("Moving for {}".format(t))
return Timed(VelocityX(0.3), t)
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()
self.progress = 0
def on_run(self, N):
new_angle = shm.kalman.roll.get()
angle_diff = new_angle - self.current_r
self.current_r = new_angle
Ad = ((angle_diff + 180) % 360) - 180
self.progress += Ad if N > 0 else -Ad
if self.progress > abs(N): self.finish()
pv = shm.settings_roll.kP.get()
roll = lambda: Sequential(
FunctionTask(lambda: shm.settings_roll.kP.set(.6)),
#*([RelativeToInitialRoll(90)] * 8),
#Timed(RelativeToCurrentRoll(90), 6),
MasterConcurrent(RollDegrees(720 - 45), RelativeToCurrentRoll(90),
VelocityX(.35)),
#MasterConcurrent(RollDegrees(360), RelativeToCurrentRoll(90), VelocityX(.35)),
#Timer(.5),
#MasterConcurrent(RollDegrees(360 - 45), RelativeToCurrentRoll(90), VelocityX(.35)),
#MasterConcurrent(RollDegrees(-720 + 45), RelativeToCurrentRoll(-90)),
FunctionTask(lambda: shm.settings_roll.kP.set(pv)),
Concurrent(
Roll(0),
VelocityX(0),
),
Timer(2),
)
t2 = Timed(RelativeToCurrentRoll(-90), 3)
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),
)))
While(
lambda: Sequential(
FunctionTask(lambda: shm.navigation_desires.heading.set(
(shm.kalman.heading.get() - .95 * math.degrees(
heading_sub_degrees(
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),
