def on_run(self, speed):
if self.update():
VelocityX(0)()
self.finish()
return
Heading(self.get_target_heading())()
VelocityX(self.calc_speed(speed))()
def __init__(self,
target_validator,
collision_validator,
locator_task,
concurrent_task=NoOp(),
ram_speed=None,
*args,
**kwargs):
"""
target_validator - a function that returns True when a target is
visible and False otherwise.
collision_validator - a function that returns True when a collision is
made and False otherwise.
concurrent_task - an optional argument for a task to run while moving
forward to ram the target. It may be used to continually align with
the target while ramming it.
ram_speed - a function that returns a speed at which to ram the target
"""
super().__init__(*args, **kwargs)
# self.logv("Starting {} task".format(self.__class__.__name__))
self.target_validator = target_validator
self.collision_validator = collision_validator
self.ram_speed = ram_speed
self.ram_task = VelocityX()
self.locator_task = locator_task
self.concurrent_task = concurrent_task
self.commit_task = Sequential(VelocityX(1), Timer(1), VelocityX(0))
self.ram_commit_phase = False
self.TIMEOUT = 25
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 __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
class Forward(FiniteTask):
def firstrun(self):
self.go = VelocityX()
self.start = time.time()
def run(self):
self.go(.7)
if self.this_run - self.start > 6:
self.go(0)
self.go._finish()
self._finish()
class Forward(Task):
def on_first_run(self):
self.go = VelocityX()
self.start = time.time()
def on_run(self):
self.go(.7)
if self.this_run_time - self.start > 6:
self.go(0)
self.go.finish()
self.finish()
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 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 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 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, *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 approach_right_passageway_task():
align_task = align_right_width()
return FinishIf(
condition=lambda: gate_elems() == 0 or
(align_task.finished and align_task.success),
task=Conditional(
main_task=FunctionTask(lambda: gate_elems() == 1),
on_success=Concurrent(
focus_elem(lambda: shm.gate.leftmost_x, offset=right_offset),
VelocityX(0.2)),
on_fail=Conditional(
main_task=FunctionTask(lambda: gate_elems() >= 2),
on_success=Concurrent(
# Align distance in between the right and middle poles
align_task,
# Align to middle of right and middle poles
PIDLoop(input_value=lambda: (shm.gate.rightmost_x.get() +
shm.gate.middle_x.get()) / 2
if shm.gate.rightmost_visible.get() else
(shm.gate.leftmost_x.get() + shm.gate.middle_x.get(
)) / 2,
target=lambda: shm.gate.img_width.get() / 2,
p=0.3,
deadband=lambda: shm.gate.img_width.get() * 0.05,
output_function=RelativeToCurrentHeading(),
negate=True),
finite=False,
),
on_fail=NoOp(),
finite=False),
finite=False))
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, 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 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, *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 Backup(speed=0.2):
return Sequential(
Timeout(SearchFor(
search_task=While(lambda: VelocityX(-speed), True),
visible=lambda: visible() and size() < ALIGN_SIZE,
consistent_frames=(1.7*60, 2.0*60),
), 15),
Zero(),
)
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 on_first_run(self, *args, **kwargs):
self.base_heading = shm.kalman.heading.get()
self.side = 0
self.loop = 1
self.heading_task = self.make_turn()
self.heading_task()
self.movement_task = None
self.stop_task = VelocityX(0)
self.state = "h"
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 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 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 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, shm_group, *args, **kwargs):
FUNNEL_DEPTH = settings.approach_funnel_depth
forward_target_task = ForwardTarget(
point=(shm_group.center_x.get, shm_group.center_y.get),
target=norm_to_vision_forward(0.0, 0.4),
depth_bounds=(.45, 1.0),
deadband=norm_to_vision_forward(-0.9, -0.9),
px=0.0004,
py=0.0008,
max_out=.05,
)
heading_target_task = HeadingTarget(
point=(shm_group.center_x.get, shm_group.center_y.get),
target=norm_to_vision_forward(0.0, 0.4),
depth_bounds=(.45, 1.0),
deadband=norm_to_vision_forward(-0.9, -0.9),
px=0.04,
py=0.0008,
max_out=(5, 0.05),
)
self.use_task(
Sequential(
cons(Depth(FUNNEL_DEPTH)),
cons(
heading_target_task,
debug=True
),
cons(
Concurrent(
forward_target_task,
PIDLoop(
input_value=shm_group.area.get,
target=9000,
deadband=250,
output_function=VelocityX(),
reverse=True,
p=0.00008,
max_out=.05,
),
finite=False,
),
total=2.5*60,
success=2.5*60*0.9,
debug=True,
),
)
)
def __init__(self, validator, forward=True, *args, **kwargs):
"""
validator - a function that returns True when a buoy is found and False
otherwise.
forward - determines whether the submarine should move forward or
backward 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.forward = forward
self.surge_task = VelocityX()
self.zero_task = Zero()
self.TIMEOUT = BUOY_SURGE_TIMEOUT
def on_first_run(self,
current_size,
current_x,
target_size,
depth_bounds=(None, None),
*args,
**kwargs):
self.pid_loop_x = PIDLoop(output_function=RelativeToCurrentHeading(),
negate=True)
self.pid_loop_x_vel = PIDLoop(output_function=VelocityX(), 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 = 8
self.py_default = 0.8
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.go = VelocityX()
self.start = time.time()
def firstrun(self):
self.go = VelocityX()
self.start = time.time()
