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 Classify():
return VisionTask(lambda vision: Sequential(
FunctionTask(lambda: shm.vision_modules.Debug.set(1)),
Timer(2),
FunctionTask(vision.classify_tubes),
Infinite(Timer(1)),
))
def on_first_run(self, vision, tube, amlan):
self.use_task(
Sequential(
Timeout(
AlignAmlan(
vision,
lambda: vision.obj_with_id(vision.tubes, tube),
amlan,
Altitude(constants.tube_dive_altitude,
overshoot_protect=True,
deadband=0.07),
align_during_depth=True,
), self.ALIGN_TIMEOUT),
Log('Moving to tube grab altitude'),
Conditional(
Timeout(Altitude(constants.tube_grab_altitude),
self.GRAB_ALTITUDE_TIMEOUT),
on_fail=Log(
'Warning: grab altitude not fully reached, grabbing anyway'
),
),
Timer(1),
amlan.Extend(),
Timer(1.5),
Log('Plucking tube'),
Timeout(RelativeToInitialDepth(-0.25, error=0.10), 10),
))
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 gen_pipe(s, a1, a2):
t1 = PipeAlign(a1, math.pi / 2, .2, DEADBAND * 1.5)
t2 = PipeAlign(a1, math.pi / 2, 0, DEADBAND * 1.5)
t3 = PipeAlign(a2, -math.pi / 2, 0, DEADBAND)
t4 = PipeAlign(a2, -math.pi / 2, .3, DEADBAND)
return Sequential(Log(s), Log("t1"), t1, Timer(.5), Log("t2"), t2,
Timer(.15), Log("t3"), t3, Timer(.5), Log("t4"), t4,
Timer(.5), Log("done"))
def new_task(self):
self.offset_len += self.stride
new_pos = self.initial_pos + self.direction_vector * self.offset_len
self.task = Sequential(
self.Scan(self.initial_heading + self.heading_amplitude),
Timer(1.0),
self.Scan(self.initial_heading - self.heading_amplitude),
Timer(1.0),
Heading(self.initial_heading),
self.MoveIncrement(new_pos[0], new_pos[1]),
)
def on_first_run(self, vision, amlan, blind=False):
initial_tube = amlan.shm_tube.get()
def apply_drop():
# Add tube to table
table_tubes = get_shm_array(shm.recovery_world_table, 'has_tube')
table_tubes[initial_tube] = True
set_shm_array(shm.recovery_world_table, 'has_tube', table_tubes)
self.use_task(
Sequential(
AlignAmlan(
vision,
lambda: vision.obj_with_id(vision.ellipses,
amlan.shm_tube.get()),
amlan,
FastDepth(constants.ellipse_depth),
blind=blind,
align_during_depth=True,
),
Sequential(
Log('Moving to tube drop depth'),
GradualDepth(constants.drop_depth),
) if not blind else NoOp(),
Timer(1),
amlan.Retract(),
FunctionTask(apply_drop),
Sequential(
Log('Moving up away from table slowly'),
GradualDepth(constants.drop_depth + self.AFTER_DROP_DELTA),
) if not blind else NoOp(),
))
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 approach(self, *args, **kwargs):
# print("approach")
x = uniform(0.0, 1.0)
if .9 < x < .95:
print("Lost")
self.SearchTimer = Timer(1)
return "search"
elif .95 < x < .97:
print("Uncentered")
self.TargetTimer = Timer(1)
return "target"
elif .97 < x:
print("Approached!")
return "ram"
def Extend(self):
# return SetActuators([self.extend_piston], [self.retract_piston])
return Sequential(
Log('Extending {} Amlan'.format(self.name)),
SetActuators([], [self.retract_piston]),
Timer(0.3),
SetActuators([self.extend_piston], []),
)
def Retract(self):
# return SetActuators([self.retract_piston], [self.extend_piston])
return Sequential(
# THE THIRD PISTON STATE
Log('Retracting {} Amlan'.format(self.name)),
SetActuators([self.extend_piston, self.retract_piston], []),
Timer(1),
SetActuators([self.retract_piston], [self.extend_piston]),
self._RemoveTube())
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 on_first_run(self, vision, single_bin, *args, **kwargs):
self.use_task(Conditional(
Sequential(
MoveAboveBins(vision),
Timer(0.5), # Wait for vision to stabilize
FunctionTask(lambda: vision.classify(single_bin)),
),
Log('Bins classified'),
Fail(Log('Failed to classify bins')),
))
def on_first_run(self, timeout=60, *args, **kwargs):
self.use_task(Timeout(Sequential(
# Pause initially to give object-identifying tasks time to check current state
Timer(0.5),
SpiralSearch(
relative_depth_range=0,
optimize_heading=True,
meters_per_revolution=1,
min_spin_radius=1,
deadband=0.2,
)
), timeout))
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, 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, 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 one_path(grp):
return Timeout(
90,
Sequential(
Zero(),
search_task(),
While(
lambda: Sequential(
Zero(),
Log('Centering on path'),
center(grp),
Log('Going to follow depth'),
Depth(PATH_FOLLOW_DEPTH),
Log('Aligning with path'),
Heading(pathAngle(grp.get()), deadband=0.1),
Zero(),
Timer(1),
Log(grp.angle.get()),
), lambda: checkNotAligned(grp.get())),
Log('aligned'),
Zero(),
))
def on_first_run(self, *args, **kwargs):
self.killed = shm.switches.hard_kill.get()
self.use_task(Sequential(
# VisionFramePeriod(0.1), # reset this
FunctionTask(lambda: shm.switches.soft_kill.set(1)),
FunctionTask(lambda: shm.deadman_settings.enabled.set(False)),
Log('Disabling Record vision module'),
FunctionTask(lambda: shm.vision_modules.Record.set(0)),
#AllLeds('orange'),
#Log('Wating for alignment...'),
#WaitForUnkill(wait=1.0),
#ZeroHeading(),
#Log('Aligned heading!'),
#AllLeds('cyan'),
# Need a swimmer to do this
Log('Waiting for re-kill...'),
WaitForUnkill(killed=False, wait=0.5),
#AllLeds('blue'),
Log('Waiting for unkill signal to start mission...'),
WaitForUnkill(wait=5.0),
Timer(5),
Log('Starting mission!'),
#AllLeds('red'),
Log('Zeroing'),
# ZeroHeading(),
Zero(),
FunctionTask(lambda: shm.switches.soft_kill.set(0)),
# EnableController(),
# Heading(0), # This will revert to the aligned heading
Log('Enabling Record vision module'),
FunctionTask(lambda: shm.vision_modules.Record.set(1)),
))
def __init__(self,
location_validator,
target_coordinates,
vision_group,
collision_validator,
ram_concurrent_task=NoOp(),
first_buoy=False,
right=True,
yellow=False,
*args,
**kwargs):
super().__init__(*args, **kwargs)
self.logv("Starting {} task".format(self.__class__.__name__))
self.location_validator = location_validator
self.target_coordinates = target_coordinates
self.collision_validator = collision_validator
self.ram_concurrent_task = ram_concurrent_task
self.heading_task = HeadingRestore()
if first_buoy:
self.locator_task_a = LocateAlignedBuoy(self.location_validator,
forward=True)
else:
self.locator_task_a = LocateAdjacentBuoy(self.location_validator,
right=right)
self.locator_task_b = LocateAlignedBuoy(self.location_validator,
forward=False)
self.align_task = AlignTarget(self.location_validator,
self.locator_task_a,
self.target_coordinates, vision_group,
self.heading_task)
self.ram_task = RamTarget(
self.location_validator,
self.collision_validator,
# LocateBuoy(self.location_validator,checkBehind=True),
self.locator_task_b,
self.ram_concurrent_task)
if yellow:
self.forward_task = DirectionalSurge(6, .5)
self.retreat_task = DirectionalSurge(4, -.5)
else:
self.forward_task = DirectionalSurge(constants.FORWARD_TIME, .2)
self.retreat_task = DirectionalSurge(constants.BACKUP_TIME, -1)
self.depth_task = DepthRestore()
self.tasks = Sequential(
Zero(),
SeqLog("Restoring Depth"),
self.depth_task,
SeqLog("Locating Buoy"),
self.locator_task_a,
SeqLog("Aligning"),
self.align_task,
SeqLog("Approaching"),
self.ram_task,
Zero(),
Timer(.5),
)
# self.tasks = Sequential(Zero(), self.depth_task, self.locator_task, self.align_task,
# self.ram_task, self.forward_task, self.retreat_task,
# self.heading_task
# )
self.TIMEOUT = 90
def on_first_run(self, task_class, *args, **kwargs):
self.vision = Vision()
task = task_class(self.vision, *args, **kwargs)
self.task = Sequential(Timer(1), task)
def __init__(self,
location_validator,
target_coordinates,
vision_group,
collision_validator,
ram_concurrent_task=NoOp(),
first_buoy=False,
right=True,
yellow=False,
*args,
**kwargs):
"""
location_validator - a function that returns True when the target has
been found and False otherwise
target_coordinates - a tuple representing the coordinates of the target
in the xz-plane
vision_group - the shm group for the buoy
collision_validator - a function that returns True when there has been
a collision with the target and False otherwise.
ram_concurrent_task - an optional task to run concurrently when ramming
the target
"""
super().__init__(*args, **kwargs)
self.logv("Starting {} task".format(self.__class__.__name__))
self.location_validator = location_validator
self.target_coordinates = target_coordinates
self.collision_validator = collision_validator
self.ram_concurrent_task = ram_concurrent_task
self.heading_task = HeadingRestore()
if first_buoy:
self.locator_task_a = LocateAlignedBuoy(self.location_validator,
forward=True)
else:
self.locator_task_a = LocateAdjacentBuoy(self.location_validator,
right=right)
self.locator_task_b = LocateAlignedBuoy(self.location_validator,
forward=False)
self.align_task = AlignTarget(self.location_validator,
self.locator_task_a,
self.target_coordinates, vision_group,
self.heading_task)
self.ram_task = RamTarget(
self.location_validator,
self.collision_validator,
# LocateBuoy(self.location_validator,checkBehind=True),
self.locator_task_b,
self.ram_concurrent_task)
if yellow:
self.forward_task = DirectionalSurge(6, .5)
self.retreat_task = DirectionalSurge(3, -.5)
else:
self.forward_task = DirectionalSurge(constants.FORWARD_TIME, .2)
self.retreat_task = DirectionalSurge(constants.BACKUP_TIME, -1)
self.depth_task = DepthRestore()
self.tasks = Sequential(Zero(),
SeqLog("Restoring Depth"), self.depth_task,
SeqLog("Locating Buoy"), self.locator_task_a,
SeqLog("Aligning"), self.align_task,
SeqLog("Approaching"), self.ram_task, Zero(),
Timer(.5),
SeqLog("Ramming"), self.forward_task,
SeqLog("Backing up"), self.retreat_task,
SeqLog("Restoring heading"), self.heading_task)
# self.tasks = Sequential(Zero(), self.depth_task, self.locator_task, self.align_task,
# self.ram_task, self.forward_task, self.retreat_task,
# self.heading_task)
self.TIMEOUT = 90
shm.recovery_crucifix.offset_y.get())
def angle_offset_crucifix():
return shm.recovery_crucifix.angle_offset.get()
def size_crucifix():
return shm.recovery_crucifix.size.get()
GrabCrucifix = lambda: \
Sequential(
Search(visible_crucifix),
Center(center_crucifix, visible_crucifix),
Align(center_crucifix, angle_offset_crucifix, visible_crucifix),
Center(offset_crucifix, visible_crucifix, db=10),
MasterConcurrent(
Sequential(
Timer(15),
_Grab()),
RelativeToCurrentDepth(DESCEND_DEPTH, error=0.2),
),
Depth(SEARCH_DEPTH, error=0.2),
Timeout(Consistent(visible_crucifix, count=1.5, total=2.0, invert=True, result=True), 10),
Log('crucifix grabbed successfully'),
)
SearchCrucifix = lambda: Search(visible_crucifix)
CenterCrucifix = lambda: Center(center_crucifix, visible_crucifix)
FollowPipe = lambda h1, h2: Sequential(
PipeAlign(h1),
Zero(),
Log("Aligned To Pipe!"),
DownwardTarget(lambda: (shm.path_results.center_x.get(),
shm.path_results.center_y.get()),
target=(0, 0),
deadband=(.1, .1),
px=0.5,
py=0.5),
Zero(),
Log("Centered on Pipe!"),
FunctionTask(lambda: shm.navigation_desires.heading.set(
-180 / 3.14 * h2.get() + shm.kalman.heading.get())),
Timer(4),
Log("Facing new direction!"),
Zero(),
)
FullPipe = lambda bend_right=False: Sequential(
# Don't do anything stupid
FunctionTask(lambda: shm.path_results.num_lines.set(0)),
BigDepth(settings.depth),
Zero(),
Log("At right depth!"),
Retry(
task_func=lambda: Sequential(
Log("Searching for path..."),
SearchTask(),
Zero(),
def on_first_run(self, *args, **kwargs):
#heading = Heading((kalman.heading.get() + 180) % 360, error=1)
return Sequential(Pitch(0, error=1), Roll(0, error=1), Timer(1.5), Heading(lambda: kalman.heading.get() + 180, error=1), Timer(1))
def maybe_add_roll(self, original, roll_direction):
tasks = [original]
if self.roll_extension:
roll = -roll_direction * 45
tasks.extend([Roll(roll, error=10), Timer(1.0), Roll(0, error=3)])
return Sequential(*tasks)
def on_run(self, function):
function()
self.finish()
def reset_counter():
global counter
counter = 0
def counter():
global count
count += 1
PickUp = Sequential(
Depth(DEPTH_UP),
Zero(),
Functioner(reset_counter),
*([
Functioner(counter),
Log('Picking up {}'.format(count)),
Timer(DELAY_UP),
Depth(DEPTH_DOWN),
Timer(DELAY_DOWN),
Depth(DEPTH_UP),
] * ATTEMPTS),
Depth(DEPTH_UP),
Zero(),
)
def __init__(self, axis_task, *args, **kwargs):
super().__init__(*args, **kwargs)
self.axis_task = axis_task
self.timer_task = Timer(ROTATION_TIME / ROTATION_SEGMENTS)
# GrabVampire(),
ReleaseVampire(lambda: edge(crucifix_task())),
Conditional(FunctionTask(get_crucifix_found), on_success=\
Sequential(
GoToMarker('crucifix'),
GrabCrucifix(),
GoToMarker(crucifix_task),
)
)
)
Mark = lambda: SetMarker('center')
Test = lambda: Sequential(
Mark(),
Timer(20),
Recovery())
def reflect():
markers = PositionMarkers()
point1 = markers.get('center')
point2 = markers.get('first')
if point1 is not None and point2 is not None:
x = point1[0] - (point2[0] - point1[0])
y = point1[1] - (point2[1] - point1[1])
markers.set('second', (x, y))
def edge(ed):
markers = PositionMarkers()
center = markers.get('center')
vampire = markers.get(ed)
self.finish(success=success)
else:
self.finished = False
task()
pv = shm.settings_roll.kP.get()
rolly_roll = lambda:\
Sequential(
FunctionTask(lambda: shm.settings_roll.kP.set(.6)),
MasterConcurrent(
RollDegrees(360 * 2 - 180),
RelativeToCurrentRoll(90),
VelocityX(0)
),
Timer(1),
FunctionTask(lambda: shm.settings_roll.kP.set(pv)),
Roll(0, error=10)
)
def focus_elem(elem_x, offset=0):
return HeadingTarget(
point=[lambda: elem_x().get(), 0],
target=lambda: [shm.gate.img_width.get() / 2 + offset, 0],
px=0.3,
deadband=(20, 1))
focus_left = lambda: focus_elem(lambda: shm.gate.leftmost_x)
focus_middle = lambda: focus_elem(lambda: shm.gate.middle_x)
