def TimedLog(msg, t=0.02): return Sequential(Timer(t), Log(msg)) def TestSequential(success):
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)
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 on_first_run(self, time, task, *args, **kwargs): self.timer = Timer(time)
def fake_move_y(d): v = 0.2 if d < 0: d *= -1 v *= -1 return Sequential(MasterConcurrent(Timer(d / v), VelocityY(v)), Zero())
def on_first_run(self, *args, **kwargs): self.SearchTimer = Timer(3) self.TargetTimer = Timer(4) self.RamTimer = Timer(1) self.PassTimer = Timer(.5)
Log('Dead reckoning through gate...'), Timed(VelocityX(0.4), dist1), # MoveXRough(20), VelocityX(0), BigDepth(1.5), Log('Turning toward pinger tasks...'), Heading(37 if RIGHT_HANDED else -37), Log('Dead reckoning a little bit toward pinger tasks...'), Timed(VelocityX(0.4), dist2), VelocityX(0), finite=True) SurfaceCashIn = lambda: Sequential( Zero(), Log('Surfacing at cash-in'), Timer(1.3), BigDepth(0), Timer(1.3), BigDepth(1.2), ) gate = MissionTask( name='Gate', cls=Gate, modules=[shm.vision_modules.BicolorGate], surfaces=False, timeout=timeouts['gate'], ) gate_dead_reckon = MissionTask( name='GateDead',
def FireActuators(actuator_names, duration): return Sequential(SetActuators(on_triggers=actuator_names), Timer(duration), SetActuators(off_triggers=actuator_names))
abs(shm.path_results.center_y.get() - v[1]) < deadband # def on_run(self, error, p=0.0005, i=0, d=0.0, db=0.01875, max_out=0.5, negate=False, *args, **kwargs): FollowLine = Concurrent( PIDSway(shm.path_results.center_x.get, negate=True, db=0, p=1.2, i=.05), 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),
from mission.framework.combinators import Concurrent, Sequential from mission.framework.primitive import Depth, Heading, Roll from mission.framework.timing import Timer seq = Sequential(Depth(0), Heading(180), Roll(180), Timer(2)) con = Concurrent(Depth(1), Heading(90), Roll(-90), Timer(2))
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, *args, **kwargs): self.timer_task = Timer(5) self.timer_task( ) # FIXME: This is because without a run, no parameter is passed? print("First run called with args {} and kwargs {}.".format( args, kwargs))
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)
# num here refers to the shm group, not the tracked num SearchBuoy = lambda num, count, total: Sequential( BigDepth(BUOY_DEPTH), Either( SearchFor( ForwardSearch(forward=settings.search_forward, stride=settings.search_stride, speed=settings.search_speed), shm_vars[num].visible.get, consistent_frames=(count * 60, total * 60 ) # multiple by 60 to specify in seconds ), # Time out if we can only see one die Sequential( Timer(settings.search_default_zero_timeout), Consistent(lambda: shm_vars[0].visible.get(), count=count, total=total, result=True, invert=False), ), ), ) BackUpUntilVisible = lambda num, speed, timeout: Conditional( Defer( main_task=Either( Sequential( # Get at least a little bit away first FakeMoveX(dist=-0.3, speed=0.2),
from mission.constants.timeout import timeouts def time_left(): # TODO test this? #time_in = Master.this_run_time - Master.first_run_time return 0 #20 * 60 - time_in WaitForTime = Sequential( Zero(), Log('Waiting until five minutes left...'), Log('Time left: {}, so waiting {} seconds.'.format( time_left(), max(time_left() - 5 * 60, 0))), # Wait until five minutes left Timer(max(time_left() - 5 * 60, 0)), ) CASH_IN_GROUPS = [(group.center_x, group.center_y) for group in [ shm.recovery_vision_downward_bin_red, shm.recovery_vision_downward_bin_green ]] cash_in_center = lambda: tuple( sum([val.get() for val in dimen]) for dimen in CASH_IN_GROUPS) # TODO test this SurfaceAtCashIn = Sequential( Zero(), Timer(3), Log('Aligning with cash-in'),
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
px=1, py=1, ix=.05, iy=.05), While( lambda: Sequential( FunctionTask(lambda: shm.navigation_desires .heading.set((shm.kalman.heading.get( ) - .95 * math.degrees( heading_sub_degrees( trg, heading(), math.pi * 2))) % 360) if shm.path_results.num_lines.get( ) == 2 else None), #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: abs( heading_sub_degrees(trg, heading(), math.pi * 2) ) > math.radians(5) ) #, count=6, total=8, invert=False, result=True), )), lambda: not is_done( heading, trg, dst, deadband ) #lambda: abs(heading_sub_degrees(trg, heading(), math.pi*2)) > math.radians(5) or abs(shm.path_results.center_x.get()) > .08 or abs(shm.path_results.center_y.get()) > .08 ), #, count=3, total=4, invert=False, result=True), #While(lambda: Log("V: {} h: {} d: {} x: {} y: {} c: {}".format(shm.path_results.num_lines.get(), heading(), math.degrees(heading_sub_degrees(trg, heading(), math.pi*2)), shm.path_results.center_x.get(), shm.path_results.center_y.get(), heading_to_vector(heading()) * dst)), lambda: True), #While(lambda: Log(s(int(math.degrees(heading_sub_degrees(trg, heading(), math.pi*2))) + 180)), lambda: True), #While(lambda: Log(s2(int(math.degrees(trg)), int(math.degrees(heading()))) if shm.path_results.num_lines.get() == 2 else 'X'), lambda: True), ),
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
set_gate = lambda: MissionTask(name="SetGate", cls=lambda: SetMarker('gate'), modules=[], surfaces=False, timeout=20) goto_gate = lambda: MissionTask(name="GoToGate", cls=lambda: GoToMarker('gate'), modules=[], surfaces=False, timeout=60) path = lambda: MissionTask( name="path", cls=lambda: Sequential(Timer(5), RelativeToInitialHeading(-45), Timer(5), MoveX(8, deadband=0.2)), modules=[], surfaces=False, timeout=30) tasks_nonrandom = [ lambda: gate, path, set_gate, track_pinger, lambda: surface, goto_gate, search_torpedoes, ]
def on_first_run(self, seconds, function, value, *args, **kwargs): self.timer = Timer(seconds)
close_to = lambda point1, point2, db=20: abs(point1[0]-point2[0]) < db and abs(point1[1]-point2[1]) < db Align = lambda centerf, anglef, visiblef, closedb=10, aligndb=7: Sequential( Log('Aligning'), MasterConcurrent( Consistent(lambda: close_to(centerf(), CAM_CENTER) and abs(anglef()) < aligndb, count=2.3, total=3, invert=False, result=True), While(lambda: Consistent(visiblef, count=2.5, total=3.0, invert=True, result=False), True), While(lambda: Center(centerf, visiblef), True), PIDHeading(anglef, p=0.47), AlwaysLog(lambda: 'align %s' % str(anglef()))), Zero()) _Grab = lambda: SetActuators(on_triggers=['manipulator_grab']) _Release = lambda: Sequential( SetActuators(on_triggers=['manipulator_release'], off_triggers=['manipulator_grab']), Timer(0.3), SetActuators(off_triggers=['manipulator_release'])) GrabVampireOpenCoffin = lambda: \ Sequential( Search(visible_open), Center(center_open, visible_open, db=20), SetMarker('before_grab'), Align(centerf=center_open, anglef=angle_offset_open, visiblef=visible_open), Center(offset_open, visible_open, db=10), MasterConcurrent( Sequential( Timer(15), _Grab()), RelativeToCurrentDepth(DESCEND_DEPTH, error=0.2), ),
get_wolf_center, lambda: heading_to_vector(shm.bins_status.wolf_angle.get( )), shm.bins_status.wolf_angle.get, shm.bins_status.wolf_visible.get, math. pi / 2, -.2j, DEADBAND) center_bat = lambda: PipeAlign( get_bat_center, lambda: heading_to_vector(shm.bins_status.bat_angle.get( )), shm.bins_status.bat_angle.get, shm.bins_status.bat_visible.get, math.pi / 2, -.02 - .15j, DEADBAND) cb = center_bat() full_wolf = lambda: Sequential( Log("Targeting wolf for drop"), Depth(1), center_wolf(), Depth(2.3), center_wolf(), Timer(.4), FireActuator('right_marker', 0.25), FireActuator('left_marker', 0.25), # do dropper stuff ) full_bat = lambda: Sequential( Log("Targeting bat for drop"), Depth(1), center_bat(), Depth(2.3), center_bat(), Timer(.4), FireActuator('right_marker', 0.25), FireActuator('left_marker', 0.25), # do dropper stuff
def startup(): shm.switches.hard_kill.set(0) shm.switches.soft_kill.set(0) def shutdown(): shm.switches.soft_kill.set(1) Qualify = Sequential( RelativeToInitialHeading(0), FunctionTask(startup), Depth(1), Timer(2), Depth(2), Timer(2), Depth(2.5), Timer(2), # Castor InterMoveX(8), Depth(0.8), InterMoveX(7), # # Pollux # InterMoveX(3), # Depth(1.5), # InterMoveX(4), RelativeToInitialHeading(90),
def PipeAlign(get_center, heading_vect, heading, get_visible, trg, dst, deadband, angle_range=math.pi * 2, abs_offset=(0, 0)): return Sequential( Log("PipeAlign start"), MasterConcurrent( Consistent(lambda: is_done(heading_vect, heading, get_center, trg, dst, deadband, abs_offset), count=.5, total=.75, invert=False, result=True), While( lambda: Sequential( #Log("attempt asdasd"), Concurrent( DownwardTarget( lambda: get_center(), target=lambda: (np.float64([heading_vect()]).view(np.complex128) * -dst).view(np.float64)[0] + abs_offset, deadband=(deadband, deadband), px=2, py=2, dx=.5, dy=.5 #, ix=.15, iy=.15 ), While( lambda: Sequential( FunctionTask( lambda: shm.navigation_desires.heading.set( (shm.kalman.heading.get( ) - .95 * math.degrees( heading_sub_degrees( trg, heading(), angle_range)) ) % 360) if get_visible() else None), #Log('{:03d} '.format(int(shm.navigation_desires.heading.get())) + s2(int(math.degrees(trg)), int(math.degrees(heading())), int(shm.desires.heading.get()))), Timer(.05)), lambda: abs( heading_sub_degrees(trg, heading(), math.pi * 2 )) > math.radians(5) ) #, count=6, total=8, invert=False, result=True), )), lambda: True #lambda: abs(heading_sub_degrees(trg, heading(), math.pi*2)) > math.radians(5) or abs(shm.path_results.center_x.get()) > .08 or abs(shm.path_results.center_y.get()) > .08 ), #, count=3, total=4, invert=False, result=True), #While(lambda: Log("V: {} h: {} d: {} x: {} y: {} c: {}".format(shm.path_results.num_lines.get(), heading(), math.degrees(heading_sub_degrees(trg, heading(), math.pi*2)), shm.path_results.center_x.get(), shm.path_results.center_y.get(), heading_to_vector(heading()) * dst)), lambda: True), #While(lambda: Log(s(int(math.degrees(heading_sub_degrees(trg, heading(), math.pi*2))) + 180)), lambda: True), #While(lambda: Log(s2(int(math.degrees(trg)), int(math.degrees(heading()))) if shm.path_results.num_lines.get() == 2 else 'X'), lambda: True), ), Log("Centered on Pipe in PipeAlign!"), #FunctionTask(lambda: shm.navigation_desires.heading.set(-180/math.pi*heading()+shm.kalman.heading.get())) )
def tasks_from_params(task, params, tup=False): return [task(*param if tup else param) for param in params] def tasks_from_param(task, param, length, tup=False): return [task(*param if tup else param) for i in range(length)] def interleave(a, b): return [val for pair in zip(a, b) for val in pair] Descend = lambda depth, timeout, pause, deadband: Sequential( Either( Timer(timeout), Depth(depth, deadband=deadband), ), Timer(pause), ) # This lets us descend in depth steps rather than all at once class BigDepth(Task): def on_first_run(self, depth, largest_step=0.5, timeout=4, pause=0, deadband=0.1): init_depth = shm.kalman.depth.get()
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() )
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)
from mission.framework.task import Task from mission.framework.combinators import Sequential from mission.framework.timing import Timer from mission.framework.primitive import FunctionTask, Log import shm from mission.constants.sub import PISTONS, TORPEDOES, PISTON_DELAY, TORPEDO_DELAY Actuate = lambda channel, value: FunctionTask(lambda: channel.set(value)) FirePiston = lambda piston: Sequential( # Open Actuate(piston[1], 0), Actuate(piston[0], 1), Timer(PISTON_DELAY), # Close Actuate(piston[0], 0), Actuate(piston[1], 1), Timer(PISTON_DELAY), # Reset Actuate(piston[1], 0), Timer(PISTON_DELAY), ) FireTorpedo = lambda torpedo: Sequential( # Fire Actuate(torpedo, 1), Timer(TORPEDO_DELAY), # Reset Actuate(torpedo, 0),
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)
def TestTimeout(): return Timeout(Timer(2), 1)