def on_first_run(self, altitude, p=0.4, d=0.1, deadband=0.04, *args, **kwargs): rel_depth = RelativeToCurrentDepth() def relative_offset_depth(out): current_altitude = shm.dvl.savg_altitude.get() rel_depth(out + math.copysign(self.RELATIVE_DEPTH_OFFSET, current_altitude - altitude)) # Remedy for depth overshoots due to bad depth control # TODO fix control to make this not necessary # TODO fix Conditional to truly work as non-finite self.go_down = PIDLoop( shm.dvl.savg_altitude.get, relative_offset_depth, target=altitude, p=p, d=d, negate=True, deadband=deadband, ) self.go_up = RelativeToCurrentDepth(-0.3)
def on_first_run(self, depth_bounds=(None, None), *args, **kwargs): self.pid_loop_x = PIDLoop(output_function=RelativeToCurrentHeading(), 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, depth_bounds=(None, None), *args, **kwargs): self.pid_loop_x = PIDLoop(output_function=VelocityY(), 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 = 0.8 self.py_default = 0.8 PositionalControl(False)()
def on_first_run(self, vision, bin_func, *args, **kwargs): self.use_task(ConsistentTask(PIDLoop( input_value=lambda: bin_func().obs.length, output_function=RelativeToCurrentDepth(), target=1.2, deadband=0.02, p=0.7, d=0.3, )))
def on_first_run(self, vision, *args, **kwargs): self.pid = PIDLoop( input_value=lambda: self.y_ratio(vision), output_function=RelativeToCurrentDepth(), target=0.75, deadband=constants.DEFAULT_DEADBAND, p=2, d=4, negate=True, )
def on_first_run(self, *args, **kwargs): self.use_task( WithQuaternionControl( Sequential( Log('Time to celebrate!'), Depth(3, error=0.2), Pitch(90, error=5), Log('Weeeeeeeeeee'), MasterConcurrent( self.CheckDepth(), RelativeToCurrentDepth(-0.25), RelativeToCurrentHeading(10), ), FunctionTask( lambda: shm.settings_control.enabled.set(False)), Zero(), )))
def stop(self): RelativeToCurrentDepth(0)() RelativeToCurrentHeading(0)()
def stop(self): VelocityY(0)() RelativeToCurrentDepth(0)()
def on_run(self, depth, error=0.08, *args, **kwargs): diff = call_if_function(depth) - shm.kalman.depth.get() relative = math.copysign(self.RELATIVE_DESIRE, diff) RelativeToCurrentDepth(relative)() if abs(diff) < error: self.finish()
def DepthGrab(): return RelativeToCurrentDepth(0.3)
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)
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), ), Depth(SEARCH_DEPTH), GoToMarker('before_grab'), UnsetMarker('before_grab'), Timeout(Consistent(visible_open, count=1.5, total=2.0, invert=False, result=True), 10), # Grab(), # ??? # Release??? ) LID_DEPTH = 0.4 LID_DEPTH_1 = 0.5 initial_depth = 3 def record_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() )