def on_run(self, *args): if self.last_run_time is None: return dt = self.this_run_time - self.last_run_time self.revolve_angle += self.degrees_per_second * dt if self.revolve_angle > 390: Heading(self.initial_heading)() Pitch(0)() Roll(0)() self.finish() return rot_2d = rotate((0, 1), self.revolve_angle) rotation_axis = np.array((rot_2d[0], rot_2d[1], 0)) conic_quat = quat_from_axis_angle(rotation_axis, self.conic_angle_rad) # TODO How can we achieve the same effect without using heading? heading, pitch, roll = conic_quat.hpr() Heading(heading)() Pitch(pitch)() Roll(roll)()
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 enter_follow(self): self.logi("Following a heading of %0.3f" % self.heading_to_pinger) self.follow_change_heading = Heading() self.follow_inital_heading = Heading(self.heading_to_pinger + heading_offset) self.follow_vel_x = VelocityX() self.follow_vel_y = VelocityY() distance_to_pinger = self.elevation_to_distance(self.follow_elevation) self.follow_vel_x(x_dir * self.get_follow_speed(distance_to_pinger)) self.follow_vel_y(0.0)
def Spin(): return Sequential( Timed(VelocityX(1), segment_time), Zero(), Heading(initial_heading + 90), Timed(VelocityY(-1), segment_time), Zero(), Heading(initial_heading + 180), Timed(VelocityX(-1), segment_time), Zero(), Heading(initial_heading + 270), Timed(VelocityY(1), segment_time), Zero(), Heading(initial_heading), )
def on_run(self, speed): if self.update(): VelocityX(0)() self.finish() return Heading(self.get_target_heading())() VelocityX(self.calc_speed(speed))()
def on_first_run(self): self.update_data() self.align = Heading( lambda: self.pipe_results.angle + shm.kalman.heading.get(), deadband=0.5) self.alignment_checker = ConsistencyCheck(19, 20) pipe_found = self.pipe_results.heuristic_score > 0 self.center = DownwardTarget(lambda self=self: (self.pipe_results.center_x * 100, self. pipe_results.center_y * 100), target=(0, 0), deadband=(1, 1), px=0.03, py=0.03, dx=0.01, dy=0.01, valid=pipe_found) self.logi("Beginning to align to the pipe's heading") c_center = "Camera Center: " + str((.5, .5)) self.logi("Pipe center" + str(self.pipe_results.center_x) + " " + str(self.pipe_results.center_y)) self.logi(c_center)
def on_run(self, meters_per_revolution=1.3, deadband=0.2, spin_ratio=1, relative_depth_range=0.0, heading_change_scale=None, optimize_heading=False, min_spin_radius=None): radius = self.calc_radius(self.theta, meters_per_revolution) target = self.calc_position(self.theta, meters_per_revolution, relative_depth_range) delta = target - _sub_position() # fake_target = target + (delta * 10) fake_target = target GoToPosition(fake_target[0], fake_target[1], depth=fake_target[2])() desired_heading = (spin_ratio * self.theta) + 90 if heading_change_scale is not None: desired_heading *= min(1.0, radius) * heading_change_scale if optimize_heading: desired_heading = math.degrees(math.atan2(delta[1], delta[0])) if min_spin_radius is not None: if (not abs_heading_sub_degrees(shm.kalman.heading.get(), desired_heading) < 10 or not radius > min_spin_radius ) and not self.started_heading: desired_heading = shm.navigation_desires.heading.get() else: self.started_heading = True Heading(desired_heading % 360)() if np.linalg.norm(_sub_position() - target) < deadband: self.theta += 10
def on_first_run(self, north, east, heading=None, depth=None, optimize=False, rough=False, deadband=0.05): self.north = north self.east = east if heading is None: self.heading = shm.navigation_desires.heading.get() else: self.heading = heading if depth is None: self.depth = shm.navigation_desires.depth.get() else: self.depth = depth self.use_task( WithPositionalControl( Concurrent(PositionN(self.north, error=deadband), PositionE(self.east, error=deadband), Heading(self.heading, error=deadband), Depth(self.depth, error=deadband)), optimize=optimize, ))
def on_first_run(self, *args, **kwargs): self.use_task( NavigationSpeed( Sequential( Depth(recovery_constants.tower_depth, error=0.08), ConsistentTask(Heading(60 * TOWARDS_BOWL_SIGN)), MoveX(30, deadband=0.08), ), 0.4))
def AvoidWall(): if not shm.recovery_world_table.know_pos.get(): return Sequential( Log('Moving away from wall'), ConsistentTask(Heading(WALL_TOWER_HEADING)), MoveXRough(2), ) else: return NoOp()
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 __init__(self, heading=None, *args, **kwargs): """ heading - a heading to use as the original heading """ super().__init__(*args, **kwargs) # Store the start heading of the sub if heading is None: self.start_heading = shm.kalman.heading.get() else: self.start_heading = heading self.heading_task = Heading(self.start_heading, error=4)
def on_first_run(self, current, desire, mod=360, *args, **kwargs): def desired_heading(): return shm.kalman.heading.get() - (heading_sub_degrees( call_if_function(desire), call_if_function(current), mod=mod)) self.use_task( Sequential( GradualHeading(desired_heading), Heading(desired_heading), finite=False, ))
def on_first_run(self, initial_heading=None, stride_speed=1, stride_time=1.5, heading_amplitude=45, *args, **kwargs): if initial_heading is None: initial_heading = shm.desires.heading.get() Heading(initial_heading)(), self.use_task( While( lambda: Sequential( Timed(VelocityX(stride_speed), stride_time), Zero(), HeadingSearch.Scan(initial_heading + heading_amplitude), HeadingSearch.Scan(initial_heading - heading_amplitude), Heading(initial_heading), ), True))
def on_first_run(self): self.update_data() self.align = Heading(lambda: self.pipe_results.angle + shm.kalman.heading.get(), deadband=0.5) self.alignment_checker = ConsistencyCheck(49, 50) pipe_found = self.pipe_results.heuristic_score > 0 self.center = DownwardTarget(lambda self=self: (self.pipe_results.center_x, self.pipe_results.center_y), target=get_downward_camera_center, deadband=(10,10), px=0.001, py=0.001, dx=0.002, dy=0.002, valid=pipe_found)
def on_first_run(self, desire, relative_desire=15, relative_deadband=25, *args, **kwargs): self.use_task( Sequential( self.GradualApproxHeading(desire, relative_desire, relative_deadband), Heading(desire, *args, **kwargs), finite=False, ))
def on_first_run(self, vision, *args, **kwargs): def AvoidWall(): if not shm.recovery_world_table.know_pos.get(): return Sequential( Log('Moving away from wall'), ConsistentTask(Heading(WALL_TOWER_HEADING)), MoveXRough(2), ) else: return NoOp() def Iteration(first_run=False): return Sequential( # Sequential( # Log('Classifying tubes'), # Retry(lambda: MoveAboveTower(vision), 3), # FunctionTask(vision.classify_tubes), # ) if first_run else NoOp(), GetTube(vision), Succeed(GetTube( vision)), # It's OK if we don't grab the second tube Surface(), AvoidWall(), # Initially classify ellipses as their size / color may change # as tubes are added Sequential( Log('Classifying ellipses'), Retry(lambda: MoveAboveTable(vision), 3), FunctionTask(vision.classify_ellipses), ) if first_run else NoOp(), PlaceTubes(vision), ) self.use_task( Except( Sequential( Retry(lambda: MoveAboveTable(vision), 1000), Log('Moving towards wall'), ConsistentTask(Heading((WALL_TOWER_HEADING + 180) % 360)), MoveXRough(2), Iteration(first_run=True), While( Iteration, lambda: count_tubes(shm.recovery_world_tower) > 0, ), ), Fail(Log('Global timeout, done trying recovery T_T')), GlobalTimeoutError, ))
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, reset_state=True, *args, **kwargs): # Limit navigation speed to prevent merge cutting out computer nav_speed = shm.navigation_settings.max_speed orig_speed = nav_speed.get() nav_speed.set(0.5) if reset_state: for loc in ['table', 'tower']: g_name = getattr(shm, 'recovery_world_{}'.format(loc)) g = g_name.get() g.know_pos = False for i in range(self.MAX_COLORS): setattr(g, 'has_tube{}'.format(i), loc == 'tower' and i < len(constants.colors)) g_name.set(g) for a in AMLANS: a.shm_tube.set(-1) self.use_task( Conditional( Defer( Sequential( Log('Starting recovery!'), AMLANS[0].FastRetract(), AMLANS[1].FastRetract(), TryRecovery(vision), ), Sequential( AMLANS[0].FastRetract(), AMLANS[1].FastRetract(), Log('Returning to pre-recovery depth'), FastDepth(shm.kalman.depth.get()), Log('Resetting navigation speed'), FunctionTask(lambda: nav_speed.set(orig_speed)), Log('Moving away from table'), Heading(WALL_TOWER_HEADING), MoveX(5, deadband=0.08), ), ), Log('Recovery success!'), Log('Recovery failure!'), ))
def Full(): return Sequential( Log('Doing balls! RIP octagon'), Log('Turning away from wall'), Heading(WALL_TOWER_HEADING, error=5), Log('Surfacing'), Depth(0.5, error=0.1), Timed(Depth(0), 3), Log('Going over to press octagon'), Timed(VelocityX(0.3), 11), Log('Pulling octagon underwater'), Timed(Concurrent( VelocityX(0.3), Depth(0.5), finite=False, ), 5), Log('Snapping octagon back in place'), Timed(VelocityX(-0.2), 4), VelocityX(0), Log('Well, I guess that\'s it for balls.'), )
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(), ))
return min((45 - diff) / 20, 0.4) else: print("0") return 0 def enable_hydrophones(): shm.hydrophones_settings.enabled.set(1) track = Sequential( # FunctionTask(enable_hydrophones), Depth(1), Concurrent( While(lambda: FunctionTask(update), lambda: True), While(lambda: Heading(get_target_heading), lambda: True), While(lambda: VelocityX(calc_speed), lambda: True), )) class _TrackPinger(Task): def on_first_run(self, speed=0.4): self.last_shmval = None self.last_target_heading = None self.last_target_elevation = 0 self.checker = ConsistencyCheck(count=3, total=7) def update(self): self.shmval = shm.hydrophones_results_track.tracked_ping_heading.get() # print('last: ' + str(self.last_shmval) + ', new: ' + str(self.shmval)) if self.last_shmval is None or self.shmval != self.last_shmval:
Consistent(lambda: single_buoy_size() > BIG_SIZE_THRESH, count=0.2, total=0.3, invert=False, result=True), Succeed(CenterSingleBuoy()), Succeed(VelocityX(.3))), Log('yeet'), Timed(VelocityX(0.3), 8), Zero()) # Search then approach the buoy SearchAndApproach = lambda: Sequential(SearchCalled(), AlignCalledNormal( ), ApproachCalled()) # , AlignCalledNormal()) # The full mission for the triangular buoy TriangleOnly = lambda: Sequential( Log('Searching for buoy'), Timeout(SearchCalled(), 30), Log('Found buoy'), ApproachAny(), Log('Ramming'), RamV(), Log('Vamp_Buoy Complete'), Heading(get_heading)) # Search task for the single target buoy SearchSingle = lambda: Sequential( Log('Searching for singular buoy'), SearchFor( VelocitySwaySearch(width=3, stride=4), shm.vamp_buoy_results.jiangshi_visible.get, # lambda: shm.vamp_buoy_results.jiangshi_visible.get() and single_buoy_size() > SEARCH_SIZE_THRESH, consistent_frames=(3, 5)), Log('Singular Found'), Zero()) DeadReckonStupid = lambda: \ Sequential( # Timed(VelocityY(DIRECTION * 0.3, error=40), 3),
) from mission.missions.actuate import ( FireGreen, FireRed, ) from mission.constants.config import cash_in as settings from mission.missions.will_common import BigDepth dist1 = 45 dist2 = 52 dist3 = dist1 / 2 stupid_castor = Sequential( BigDepth(1.5), Timed(VelocityX(0.4), dist1), # MoveXRough(20), VelocityX(0), Heading(37), Timed(VelocityX(0.4), dist2), VelocityX(0), finite=True) stupid_castor_2 = Sequential(Heading(10), Timed(VelocityX(0.4), dist3), VelocityX(0), finite=True) search = SaneHeadingSearch()
def make_turn(self): h = self.base_heading + (self.side * 90) h %= 360 print("Turning to {}".format(h)) return cons(Heading(h))
print('flipped: ' + str(h)) if h != last_pinger_heading: last_sub_heading = math.radians(shm.kalman.heading.get()) last_pinger_heading = h return h + last_sub_heading def get_desired_heading(): h = shm_heading() if h < 0: h += math.pi * 2 return math.degrees(h) def get_desired_vel_x(): h = shm_heading() if -math.pi / 4 < h < math.pi / 4: return math.cos(h) * 0.3 # maybe change this function later? else: return 0 track = Sequential( Depth(1.0), While( lambda: Sequential( Heading(get_desired_heading), VelocityX(get_desired_vel_x), ), lambda: True))
def on_run(self, pitch=True, roll=True): Heading(shm.kalman.heading.get())() ZeroWithoutHeading(pitch=pitch, roll=roll)() self.finish()
Log("Picked Up Right"), ) drop_all = Sequential( drop_right, Log("Dropped Right"), drop_left, Log("Dropped Left"), ) do_it_all = Sequential( search_left, Log("Found"), pickup_all, Log("Picked Up"), cons(Heading(45)), Log("Turned to hardcoded heading"), drop_all, Log("Dropped"), ) left = make_bin_chooser(True) right = make_bin_chooser(False) center_left = cons ( DownwardTarget( point=(lambda: left().center_x.get(), lambda: left().center_y.get()), target=norm_to_vision_downward(0, 0), deadband=norm_to_vision_downward(-0.9, -0.9), px=0.0005,
(shm.gate.rightmost_x.get() + shm.gate.middle_x.get()) / 2 if shm.gate.rightmost_visible.get() else (shm.gate.leftmost_x.get() + shm.gate.middle_x.get()) / 2, target=lambda: shm.gate.img_width.get() / 2, p=0.0015, deadband=0, output_function=VelocityY(), negate=True) search_task = lambda:\ SearchFor( Sequential( # manual list of "check here first, then just StillHeadingSearch" FunctionTask(save_heading), Log('Searching for gate: using manual turning to right'), Heading(lambda: saved_heading + 90), Heading(lambda: saved_heading + 180), Log('Searching for gate: didn\'t find it - turn back'), Heading(lambda: saved_heading + 90), Heading(lambda: saved_heading + 0), Log('Searching for gate: didn\'t find it - spin'), Heading(lambda: saved_heading + 90), Heading(lambda: saved_heading + 180), Heading(lambda: saved_heading + 270), Heading(lambda: saved_heading + 0), Log('Searching for gate: fall back on StillHeadingSearch'), StillHeadingSearch() ), shm.gate.leftmost_visible.get, consistent_frames=(6, 10) )
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))