def on_first_run(self, vision, *args, **kwargs): self.pid = PIDLoop( input_value=lambda: bbar_angle(vision), output_function=VelocityY(), target=0, deadband=3, p=0.8, d=0.4, ) self.depth = Depth(constants.WIRE_DEPTH)
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, 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 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 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)
import shm results_groups = shm.bicolor_gate_vision class Consistent(Task): def on_first_run(self, test, count, total, invert, result): # Multiple by 60 to specify in seconds self.checker = ConsistencyCheck(count * 60, total * 60, default=False) def on_run(self, test, count, total, invert, result): test_result = call_if_function(test) if self.checker.check(not test_result if invert else test_result): self.finish(success=result) XTarget = lambda x, db: PIDLoop(input_value=x, target=0, output_function=VelocityY(), negate=True, p=0.4 if is_mainsub() else 0.4, deadband=db) DEPTH_TARGET = settings.depth #gate = Sequential(target, Log("Targetted"), center, Log("Centered"), charge) # This is the unholy cross between my (Will's) and Zander's styles of mission-writing gate = Sequential( Log('Depthing...'), BigDepth(DEPTH_TARGET), Log('Lining up...'), ConsistentTask(Concurrent( Depth(DEPTH_TARGET), XTarget(x=results_groups.gate_center_x.get, db=0.03), finite=False
Zero(), finite=False ) align_on_three_elem = lambda:\ Sequential( Log('aligning on three elems'), ConsistentTask( # while we CAN see all gates FinishIf( task=Concurrent( # align leftmost and rightmost by length PIDLoop( input_value=lambda: shm.gate.rightmost_len.get() / shm.gate.leftmost_len.get(), target=1, p=0.5, deadband=0.1, output_function=VelocityY() ), # align to the center of the leftmost and rightmost PIDLoop( input_value=lambda: (shm.gate.leftmost_x.get() + shm.gate.rightmost_x.get()) / 2, target=lambda: shm.gate.img_width.get() / 2, p=0.2, deadband=alignment_tolerance_fraction, output_function=RelativeToCurrentHeading(), negate=True ), finite=False ), condition=lambda: gate_elems() < 3 or is_aligned()
def on_first_run(self, *args, **kwargs): self.pid_loop = PIDLoop(output_function=RelativeToCurrentHeading())
def on_first_run(self, *args, **kwargs): self.pid_loop = PIDLoop(output_function=VelocityX())
def firstrun(self, *args, **kwargs): print("forward started") self.pid_loop_x = PIDLoop(output_function=VelocityX()) self.total_distance = 0
def firstrun(self, *args, **kwargs): self.pid_loop_y = PIDLoop(output_function=VelocityY()) self.total_distance = 0