def bump_buoy(self, buoys, depth=None): # Check if we saw all 3 buoys # found_buoys = filter(lambda x: x.found, buoys) found_buoys = buoys if found_buoys: found_buoys.sort(key=lambda x: x.theta) if len(found_buoys) == 3: print "found 3 buoys" # Assign tracking_buoy_id to middle buoy self.tracking_buoy_id = found_buoys[1].id if len(found_buoys) == 1: print "found 1 buoy" self.tracking_buoy_id = found_buoys[0].id """ elif found_buoys: for buoy in found_buoys: absolute_theta = buoy.theta + sw3.data.imu.yaw() buoy.center_distance = sw3.util.circular_distance(absolute_theta, self.start_angle, 180, -180) found_buoys.sort(key=lambda x: x.center_distance) if found_buoys[0].center_distance < BUOY_CENTER_THRESHOLD: self.tracking_buoy_id = found_buoys[0].id """ # what does this conditional do? if self.tracking_buoy_id is not None: self.delete_timer("mission_timeout") # Get tracking buoy from its id tracking_buoy = None for buoy in buoys: if buoy.id == self.tracking_buoy_id: tracking_buoy = buoy # Correct if we saw it if tracking_buoy is not None: print tracking_buoy self.last_seen = time.time() if depth: depth_routine = sw3.SetDepth(depth) else: depth_routine = sw3.RelativeDepth(-(tracking_buoy.phi) / 5) print "Adjusting depth:", -tracking_buoy.phi / 5 print "id: ", tracking_buoy.id print "Theta:", tracking_buoy.theta print "Adjusting Yaw:", DEGREE_PER_PIXEL * tracking_buoy.theta sw3.nav.do(sw3.CompoundRoutine( depth_routine, sw3.Forward(FORWARD_SPEED), sw3.RelativeYaw(DEGREE_PER_PIXEL * tracking_buoy.theta), )) # Consider ending the mission if we didn't see it elif time.time() - self.last_seen > 10: self.tracking_buoy_id = None print "Lost buoy, finishing" return True
def step(self, vision_data): buoys = [] if vision_data is not None: buoys = vision_data['buoy'].buoys if len(buoys) > 0: looking_for = 'red' if self.bumped == 1: looking_for = 'green' selected = None for buoy in buoys: if buoy.color == looking_for: selected = buoy if selected is not None: sw3.nav.do( sw3.CompoundRoutine( sw3.SetDepth(7), sw3.RelativeYaw(selected.theta), sw3.Forward(.2, 6), )) self.bumped += 1 sw3.nav.do(sw3.Forward(-.2, 6)) if self.bumped == 2: self.finish_mission() else: sw3.nav.do(sw3.Forward(.1, 1))
def init(self): self.process_manager.start_process(entities.BuoyHoughEntity, "buoy", "forward", debug=True) sw3.nav.do(sw3.SetDepth(6)) self.bumped = 0
def seek(self, bins): # print "seek" # print bins if bins and self.orientdata: sw3.nav.do(sw3.Forward(0, 1)) # print bins pos_x = math.atan2(bins[0].theta, bins[0].phi) * (180 / math.pi) print pos_x pos_rho = math.sqrt(bins[0].theta**2 + bins[0].phi**2) # sw3.nav.do(sw3.Forward(0,0)) print "center" center = sw3.CompoundRoutine(sw3.RelativeYaw(pos_x), sw3.Forward(.2), sw3.SetDepth(BIN_DEPTH), timeout=3) sw3.nav.do(center) # print pos_x # sw3.nav.do(sw3.Forward(FORWARD_SPEED, CENTER_TIME)) # print "centering" #self.orientdata *= 180/math.pi if pos_x <= CENTER_THRESH: # print "centered" #orient_angle = self.orientdata sw3.nav.do(sw3.Forward(0, 1)) # sw3.nav.do(sw3.RelativeYaw(20)) # sw3.nav.do(sw3.RelativeYaw(self.orientdata)) # sw3.nav.do(sw3.RelativeDepth(BIN_DEPTH)) self.nextState()
def init(self): # set depth to bouy level sw3.nav.do(sw3.SetDepth(DEPTH_BUMP)) time.sleep(DELAY) # startup buoy vision code self.process_manager.start_process(entities.BuoyHoughEntity, "buoy", "forward", debug=True) # Go forward sw3.nav.do( sw3.CompoundRoutine( sw3.Forward(FORWARD_SPEED), sw3.HoldYaw(), )) # Captures yaw heading for ??? self.start_angle = sw.var.get("YawPID.Heading") # starts a watchdog timer for the entire mission. After 15 seconds, # the mission will terminate, regardless of what is happening. self.set_timer("mission_timeout", 15, self.finish_mission) self.tracking_buoy_id = None # State variable for bump_buoy()
def init(self): # Set depth to hedge hight, but keep heading at same time sw3.nav.do(sw3.CompoundRoutine( sw3.Forward(0), sw3.HoldYaw(), sw3.SetDepth(HEDGE_DEPTH), )) # give some time for the dive to complete time.sleep(4) """ current_depth = sw3.data.depth() while current_depth <= HEDGE_DEPTH - 3: print "changing depth ({}) to {}".format(current_depth, HEDGE_DEPTH) current_depth = sw3.data.depth() """ # start vision self.process_manager.start_process(entities.GateEntity, "gate", "forward", debug=True) # go forward sw3.nav.do(sw3.CompoundRoutine( #sw3.HoldYaw(), sw3.Forward(FORWARD_SPEED), ))
def aim(self, target): # line up with target, see all four corners # change depth to specified color sw3.nav.do(sw3.SetDepth(TARGET_DEPTH)) # go forward until only see color self.aimed += 1 self.nextState()
def init(self): sw3.nav.do( sw3.SetDepth(DEPTH), ) time.sleep(DELAY) self.process_manager.start_process(entities.GateEntity, "gate", "forward", debug=True) sw3.nav.do(sw3.CompoundRoutine( sw3.HoldYaw(), sw3.Forward(FORWARD_SPEED), ))
def init(self): self.process_manager.start_process(entities.GateEntity, "gate", "forward", debug=True) sw3.nav.do( sw3.CompoundRoutine( sw3.Strafe(STRAFE_SPEED), sw3.SetDepth(2), sw3.HoldYaw(), ))
def state_findpath(self): # TODO: check if the second buoy was center, if so, dont do another approach riseup_routine = sw3.RelativeDepth(OVER_DEPTH) runover_routine = sw3.Forward(FORWARD_SPEED) stop_routine = sw3.Forward(0) depth_goto = sw3.SetDepth(self.depth_seen) print "findpath" sw3.nav.do( sw3.SequentialRoutine(depth_goto, riseup_routine, runover_routine)) self.finish_mission()
def init(self): sw3.nav.do(sw3.SetDepth(DEPTH_BUMP)) time.sleep(DELAY) self.process_manager.start_process(entities.BuoyHoughEntity, "buoy", "forward", debug=True) sw3.nav.do(sw3.CompoundRoutine( sw3.Forward(FORWARD_SPEED), sw3.HoldYaw(), )) self.start_angle = sw.var.get("YawPID.Heading") self.set_timer("mission_timeout", 15, self.finish_mission) self.tracking_buoy_id = None # State variable for bump_buoy()
def init(self): sw3.nav.do(sw3.SetDepth(6)) self.process_manager.start_process(entities.PizzaCornerEntity, "pizzabox", "down", debug=True) self.reference_angle = sw3.data.imu.yaw() self.highest_id = None self.states = [ "findpizza", "pickup", ] self.state_num = 0 self.state = self.states[self.state_num]
def state_findpath(self): sw3.nav.do(sw3.Forward(0)) # TODO: check if the second buoy was center, if so, dont do another approach riseup_routine = sw3.RelativeDepth(REL_OVER_DEPTH) runover_routine = sw3.Forward(FORWARD_SPEED) stop_routine = sw3.Forward(0) depth_goto = sw3.SetDepth(self.depth_seen) print "findpath" sw3.nav.do( sw3.SequentialRoutine(sw3.Forward(-0.6, 0.1), riseup_routine, sw3.SetYaw(self.reference_angle), runover_routine)) self.wait_stop_mark = 3 self.next_state()
def init(self): '''runs at start of mission ''' if self.double: self.process_manager.start_process(entities.DoublePathEntity, "path", "down", debug=True) else: self.process_manager.start_process(entities.PathEntity, "path", "down", debug=True) sw3.nav.do( sw3.CompoundRoutine([ sw3.Forward(FORWARD_SPEED), sw3.RelativeYaw(0), sw3.SetDepth(4) ])) self.reference_angle = sw.var.get("YawPID.Heading") * (pi / 180) % (2 * pi) self.state = "centering" self.set_timer("path_timeout", MISSION_TIMEOUT, self.fail_mission)
def setup(self): sw3.SetDepth(-1).start() sw3.Forward(.8).start() self.state = SearchState() return
def setup(self): sw3.SetDepth(-1).start() sw3.Forward(.4).start() self.state = YawState() return
def setup(self): self.time = time.time() sw3.SetDepth(-1).start() sw3.Forward(.3).start() self.centers = [] return
def setup(self): sw3.SetDepth(DEPTH).start() sw3.Forward(STARTSPEED).start() self.state = SearchState() return
def init(self): '''runs at start of mission ''' self.set_timer("mission_timeout", MISSION_TIMEOUT, self.mission_timeout) # stop, and go down sw3.nav.do( sw3.CompoundRoutine( sw3.Forward(0), sw3.SetDepth(START_DEPTH), )) # adjust depth before starting mission """ current_depth = sw3.data.depth() while current_depth <= START_DEPTH - 3: print "Adjusting depth ({}) to go to {}.".format(current_depth, START_DEPTH) current_depth = sw3.data.depth() """ # # start vision process self.process_manager.start_process(entities.BuoyHoughEntity, "buoy", "forward", debug=True) # ease forwards in order get a better view sw3.nav.do(sw3.Forward(INITIAL_RECON_SPEED)) # capture orientation at this point self.reference_angle = sw3.data.imu.yaw() self.tracking_id = None self.depth_seen = None self.states = [ "first_approach", "bump", #"second_approach", #ommited. we only want to bump the first buoy we see. #"bump", #"center_approach", "setup_pull_down", "wait", "pull_down", "wait", #"find_path", "disconnect", "wait", "find_path", "wait", ] self.state_num = 0 self.state = self.states[self.state_num] self.set_timer("buoy_timeout", 120, self.fail_mission) # debug parameters self.i = 0 # waiting variables self.substate = '' self.wait_stop_mark = -1 self.hidden = False
#import acoustics from mission_controller import MissionController BUOY_DEPTH = 4 # # Ordered list of tasks. Can be one of the following types: # * Mission Class - Found in ``missions.<name>Mission``. Instantiated with no # arguments. # * Nav routine - Found in ``sw3.nav.<name>``. # * Tuple - First item must be a mission class. The rest of the tuple is # passed in as arguments to the ``mission.__init__``. MISSION_ORDER = [ missions.GateMission325, # 01: gate #sw3.Forward(0.5, 3), #sw3.Forward(0.0,2), sw3.SetDepth(3, 2), #sw3.Forward(0.0, 1), missions.PathMission, #sw3.SetDepth(1), #sw3.Forward(0), #sw3.Forward(0.9,30), #sw3.RelativeYaw(0), #sw3.SetDepth(2), #sw3.Forward(0.9,90), #sw3.Forward(0.9,15), #sw3.RelativeYaw(30,1), #sw3.Forward(0.9,10), #sw3.Forward(q,1), #sw3.RelativeYaw(0),
def pickup(self): sw3.nav.do( sw3.CompoundRoutine(sw3.SetDepth(9), sw3.Forward(0, 0), sw3.SetDepth(0))) self.nextState()