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)