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 set_course(self, forward, rel_yaw, get_obj=False): """Changes the course of the robot as it's searching for the pinger. ARGS: forward: how fast to go forward rel_yaw: how much to change the relative yaw heading""" # change heading and move forward (or backward) if not get_obj: sw3.nav.do(sw3.CompoundRoutine( sw3.RelativeYaw(rel_yaw), sw3.Forward(forward), ) ) return else: set_course_routine = sw3.CompoundRoutine( sw3.RelativeYaw(rel_yaw), sw3.Forward(forward), ) ) # deliver an object to the caller return set_course_routine
def step(self, vision_data): self.mission_timeout -= 1 if not vision_data: return gate_data = vision_data['gate'] if not gate_data: return print gate_data if gate_data and gate_data.left_pole and gate_data.right_pole: gate_center = DEGREE_PER_PIXEL * (gate_data.left_pole + gate_data.right_pole) / 2 # degrees # If both poles are seen, point toward it then go forward. self.gate_seen += 1 self.gate_lost = 0 if abs(gate_center) < STRAIGHT_TOLERANCE: sw3.nav.do(sw3.CompoundRoutine([ sw3.Forward(FORWARD_SPEED), sw3.HoldYaw() ])) else: print "Correcting Yaw", gate_center sw3.nav.do(sw3.CompoundRoutine([ sw3.RelativeYaw(gate_center), sw3.Forward(0.4) ])) elif self.gate_seen >= 15: self.gate_lost += 1 if self.gate_lost > 1 or self.mission_timeout <= 0: print "Heading Locked" self.finish_mission() return
def update_axis(event): global yaw_heading angle = event.angle_radians mag = max(min(event.magnitude, 1.0), -1.0) forward = -(mag * sin(angle)) rate = (mag * cos(angle)) total = abs(forward) + abs(rate) if total == 0: yaw_heading = sw3.data.imu.yaw() sw3.nav.do( sw3.CompoundRoutine( (sw3.SetYaw(yaw_heading), sw3.Forward(forward)))) else: for_p = forward / total rate_p = rate / total total = min(total, 1.0) forward = for_p * total rate = rate_p * total sw3.nav.do( sw3.CompoundRoutine((sw3.SetRotate(rate), sw3.Forward(forward))))
def findpath(self, holes): # swim around target? turn = sw3.CompoundRoutine(sw3.RelativeYaw(TURN_ANGLE), sw3.Forward(TURN_SPEED), timeout=TURN_TIMER) finish = sw3.CompoundRoutine(sw3.RelativeYaw(-TURN_ANGLE), sw3.Forward(TURN_SPEED), timeout=TURN_TIMER) sw3.nav.do(turn) sw3.nav.do(finish) self.finish_mission()
def state_approach(self, buoy_to_bump, buoys): # TODO: include depth routine if len(buoys) == 3: buoys.sort(key=lambda x: x.theta) self.tracking_id = buoys[buoy_to_bump].id if self.depth_seen is None: self.depth_seen = sw3.data.depth() track_buoy = None for buoy in buoys: if buoy.id == self.tracking_id: track_buoy = buoy if not track_buoy: return #self.set_timer("Approach_Timeout", APPROACH_TIMEOUT, self.approach_timeout, sw3.data.imu.yaw()) # print "State: BuoyBump dist: ",track_buoy.r, " x angle from current: ",track_buoy.theta, " y angle from current: ", track_buoy.phi yaw_routine = sw3.RelativeYaw(track_buoy.theta) forward_routine = sw3.Forward(FORWARD_SPEED) stop_routine = sw3.Forward(0, 0) backup_routine = sw3.Forward(BACKWARD_SPEED) reset_routine = sw3.SetYaw(self.reference_angle) track_depth_angle = (track_buoy.phi) if abs(track_depth_angle) > DEPTH_THRESHOLD: if (track_depth_angle > 0): depth_routine = sw3.RelativeDepth(-DEPTH_UNIT) if (track_depth_angle < 0): depth_routine = sw3.RelativeDepth(DEPTH_UNIT) else: depth_routine = sw3.NullRoutine() centered = False if abs(track_buoy.theta) <= CENTER_THRESHOLD: centered = True if centered: sw3.nav.do( sw3.CompoundRoutine(forward_routine, yaw_routine, depth_routine)) ''' if abs(track_buoy.r) <= DIST_THRESHOLD: self.delete_timer("Approach_Timeout") self.next_state() ''' self.next_state() else: sw3.nav.do(sw3.CompoundRoutine(stop_routine, yaw_routine))
def step(self, vision_data): if not vision_data: return if vision_data['path']: path_data = vision_data['path'] print path_data theta_x = path_data.x * FIELD_OF_VIEW * math.pi / 180 # path_data.x is percent of fram view . multiplying them gives you theta_x theta_y = path_data.y * FIELD_OF_VIEW * math.pi / 180 # path_data.y is percent of frame view . multiplying them gives you theta d = PATH_DEPTH - sw3.data.depth( ) # depth between path and camera x = d * math.sin( theta_x ) # g1ves you the x distance from the frame center to path center y = d * math.sin( theta_y ) # gives you the y distance from the frame center to path center print "Status:Step x ", x, " y ", y if self.state == "centering": self.state_centering(x, y) if self.state == "orienting": return self.state_orienting(path_data) if vision_data['gate']: gate_data = vision_data['gate'] print gate_data if gate_data.left_pole and gate_data.right_pole: gate_center = DEGREE_PER_PIXEL * ( gate_data.left_pole + gate_data.right_pole) / 2 # degrees if abs(gate_center) < STRAIGHT_TOLERANCE: sw3.nav.do( sw3.CompoundRoutine( [sw3.Forward(FORWARD_SPEED), sw3.HoldYaw()])) else: print "Correcting Yaw", gate_center sw3.nav.do( sw3.CompoundRoutine([ sw3.RelativeYaw(gate_center), sw3.Forward(0.4) ]))
def change_heading(self, forward, rel_yaw): if abs(rel_yaw) > YAW_TOLERANCE: sw3.nav.do(sw3.CompoundRoutine( sw3.RelativeYaw(-rel_yaw), sw3.Forward(forward) ) )
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 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 orient(self, bins): if bins and self.orientdata: orient_angle = self.orientdata * (180 / math.pi) # print "forward" #center = sw3.Forward(0.5,2) #centering = sw3.nav.do(center) # sw3.nav.do(center) # sw3.nav.do(sw3.Forward(FORWARD_SPEED,1)) # sw3.nav.do(sw3.Forward(0,0)) print "orient" orient = sw3.CompoundRoutine(sw3.Forward(0, 5), sw3.RelativeYaw(orient_angle)) # sw3.nav.do(sw3.CompoundRoutine(sw3.Forward(0,0),(sw3.RelativeYaw(self.orientdata),timeout =5))) sw3.nav.do(orient) #orient.on_done(lambda z: sw3.nav.do(sw3.Forward(FORWARD_SPEED, 1))) #center.on_done(lambda b: sw3.nav.do(orient)) # sw3.nav.do(sw3.RelativeDepth(BIN_DEPTH)) # print sw3.data.imu.yaw() # print self.orientdata if (abs(abs(sw3.data.imu.yaw()) - orient_angle) <= ORIENT_THRESH): print "done orienting" orient.on_done( lambda z: sw3.nav.do(sw3.Forward(FORWARD_SPEED, 1))) self.turn_count += 1 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 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): self.mission_timeout -= 1 if not vision_data: return hedge_data = vision_data['hedge'] if not hedge_data: return print hedge_data if hedge_data: hedge_center = None if hedge_data.right_pole and hedge_data.left_pole: hedge_center = DEGREE_PER_PIXEL * ( hedge_data.left_pole + hedge_data.right_pole) / 2 # degrees elif hedge_data.center_pole is not None: hedge_center = hedge_data.center_pole self.hedge_seen += 1 self.hedge_lost = 0 if hedge_center is not None: if abs(gate_center) < STRAIGHT_TOLERANCE: sw3.nav.do( sw3.CompoundRoutine( [sw3.Forward(FORWARD_SPEED), sw3.HoldYaw()])) else: print "Correcting Yaw", hedge_center sw3.nav.do( sw3.CompoundRoutine([ sw3.RelativeYaw(hedge_center + 2), sw3.Forward(FORWARD_SPEED), ])) elif self.hedge_seen >= 5: self.hedge_lost += 1 if self.hedge_lost > 1 or self.mission_timeout <= 0: sw3.nav.do( sw3.SequentialRoutine(sw3.RelativeYaw(180), sw3.Forward(-1 * FORWARD_SPEED, 5))) time.sleep(self.mission_timeout) self.finish_mission()
def init(self): '''runs at start of mission ''' self.process_manager.start_process(entities.PathEntity, "path", "down", debug=True) sw3.nav.do(sw3.CompoundRoutine([sw3.Forward(FORWARD_SPEED)])) self.reference_angle = sw3.data.imu.yaw() * (pi / 180) % (2 * pi) self.state = "centering"
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 update_axis(event): global yaw_heading global x_activated global y_activated # x and y start out at zero, however, 0 is NOT center! When an x event # comes in, but y has not been touched, y should not react (and vice # versa). This prevents an axis form moving unless it has gotten a nonzero # value before. if event.x != 0: x_activated = True if event.y != 0: y_activated = True if not x_activated: turn_rate = 0 else: # Steering wheel goes from left 0 to right 32767. # Normalize so 0 is center and scale from -1 to 1. turn_rate = (event.x - 32767 / 2) / (32767 / 2) if not y_activated: forward_rate = 0 elif event.y < 15000: # Forward forward_rate = 1 - event.y / 15000 elif event.y > 20000: # Backward forward_rate = -1 * (event.y - 20000) / (32767 - 20000) else: forward_rate = 0 if abs(turn_rate) > 0.25: sw3.nav.do( sw3.CompoundRoutine( [sw3.SetRotate(turn_rate), sw3.Forward(forward_rate)])) else: yaw_heading = sw3.data.imu.yaw() sw3.nav.do( sw3.CompoundRoutine( [sw3.SetYaw(yaw_heading), sw3.Forward(forward_rate)]))
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 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 drop(self, bins): # print "Marker Dropped" if bins and self.orientdata: orient_angle = self.orientdata * (180 / math.pi) print "orient" orient = sw3.CompoundRoutine(sw3.Forward(0), sw3.RelativeYaw(orient_angle), timeout=5) sw3.nav.do(orient) self.drop_count += 1 print "State:", self.state self.state = self.states[0] self.state_num = 0
def sweep(self, bins): # print "sweep" # print self.orientdata sweep = sw3.Forward(FORWARD_SPEED, 1) turning = sw3.CompoundRoutine(sw3.Forward(0, TURNING_TIME), sw3.RelativeYaw(180)) turnaround = lambda: sw3.nav.do(turning) #self.id_holder = self.highest_id # iif self.x == 0: # print "nav.sweep" # sw3.nav.do(sweep) #self.x = 1 if bins: for bincount in bins: if bincount.id > self.highest_id: self.highest_id = bincount.id current_bin = None if self.orientdata is not None: # if self.highest_id > self.id_holder: # this means weve seen a new bini # print self.highest_id # print self.id_holder #self.id_holder = self.highest_id # sw3.nav.do(sw3.Forward(0,0)) # sw3.nav.do(turning) print "turning" self.set_timer("bin_timeout", TURNAROUND_TIMER, turnaround) turning.on_done(lambda y: sw3.nav.do(sweep)) # print "I turned!" # sw3.nav.do(sw3.Forward(FORWARD_SPEED)) # print "turning" self.turn_count += 1 # print self.turn_countF none for bina in bins: if bina.id == self.highest_id: current_bin = bina print current_bin.shape #self.highest_id = bina.id if current_bin: if current_bin.shape is "A" or current_bin.shape is "C": if self.dropped is "A" and current_bin.shape is "C": self.nextState() if self.dropped is "C" and current_bin.shape is "A": self.nextState() if self.dropped is "E": self.nextState() self.dropped = current_bin.shape
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)])) self.reference_angle = sw.var.get("YawPID.Heading") * (pi / 180) % (2 * pi) self.state = "centering" self.set_timer("path_timeout", 45, self.fail_mission)
def step(self, vision_data): #if not vision_data: # return ac_data = self.acoustics.get_data() if ac_data['error'] == 0: # grab data epoch = ac_data['data']['epoch'] rel_yaw = ac_data['data']['heading']['ab'] if epoch > 5: print "data is stale. will try again later" return # data is stale print rel_yaw if abs(rel_yaw) > YAW_TOLERANCE: sw3.nav.do( sw3.CompoundRoutine(sw3.RelativeYaw(-rel_yaw), sw3.Forward(0))) time.sleep(6)
def step(self, vision_data): #if not vision_data: # pick next state if self.state == 'searching': self.state_searching() elif self.state == 'verify': self.state_verify() elif self.state == 'accepted': self.state_accepted() elif not (self.state in self.states): raise ValueError("{} is not an acceptable state".format(self.state)) if abs(rel_yaw) > YAW_TOLERANCE: # change yaw and go forward sw3.nav.do( sw3.CompoundRoutine( sw3.RelativeYaw(-rel_yaw), sw3.Forward(.3) ) ) time.sleep(6)
def state_approach(self, buoy_to_bump, buoys): # self.tracking_id # if vision doesn't return any buoy objects, there's nothing to process. if not buoys: return # print "buoy to bump: {}".format(buoy_to_bump) # TODO: include depth routine if len(buoys) >= 1: # Note: this is meant to create a pocket of code that only runs at # the begginning of the approach phase. The assumption is that every # approach begins with 3 buoys in view. Once the sub moves forward, the # assumption is that this setup code ran enough times such that the # correct buoy is being tracked, and the optimal depth for seeing all # buoys simultaneously has been captured. # sort buoys from left to right buoys.sort(key=lambda x: x.theta) # store buoy-to-bump's ID under self.tracking_id for later recall self.tracking_id = buoys[buoy_to_bump].id # update depth parameter if self.depth_seen is None: self.depth_seen = sw3.data.depth() # assert that the tracked/target buoy is still in our field of view track_buoy = None for buoy in buoys: if buoy.id == self.tracking_id: track_buoy = buoy # if target buoy is not in our FOV, terminate processing and try again. # mission will time out if this happens too much. if not track_buoy: return # start/reset the approach timer self.set_timer("Approach_Timeout", APPROACH_TIMEOUT, self.approach_timeout, sw3.data.imu.yaw()) # print debug text print("approach state: {} buoys detected".format(len(buoys))) # print "State: BuoyBump dist: ",track_buoy.r, " x angle from current: ",track_buoy.theta, " y angle from current: ", track_buoy.phi # various buoy related routines yaw_routine = sw3.RelativeYaw(track_buoy.theta) forward_routine = sw3.Forward(FORWARD_SPEED) stop_routine = sw3.Forward(0, 0) backup_routine = sw3.Forward(BACKWARD_SPEED) reset_routine = sw3.SetYaw(self.reference_angle) # generate Depth changing parameters track_depth_angle = (track_buoy.phi) centered = False #print (track_depth_angle) # dynamically assign depth_routine to trim depth by DEPTH_UNITs if abs(track_depth_angle) > DEPTH_THRESHOLD: if (track_depth_angle > 0): depth_routine = sw3.RelativeDepth(-DEPTH_UNIT) if (track_depth_angle < 0): depth_routine = sw3.RelativeDepth(DEPTH_UNIT) else: depth_routine = sw3.NullRoutine() centered = True # check if yaw is centered #print (track_buoy.theta) if abs(track_buoy.theta) <= CENTER_THRESHOLD: centered = True * centered # if yaw and depth are not centered, stop and adjust yaw/depth if not centered: sw3.nav.do( sw3.CompoundRoutine(stop_routine, yaw_routine, depth_routine)) # else yaw and depth are centered, advance to the next state! else: sw3.nav.do( sw3.CompoundRoutine(forward_routine, yaw_routine, depth_routine)) self.delete_timer("Approach_Timeout") self.next_state()
def step(self, vision_data): print "timeout = {}".format(self.mission_timeout) # enable timeouts if necessary if TIMEOUT_ENABLED: self.mission_timeout -= 1 # if vision is busy processing, skip and wait for vision data. if not vision_data: return gate_data = vision_data['gate'] if not gate_data: return print gate_data if gate_data and gate_data.left_pole and gate_data.right_pole: # capture the location of the gate center in the camera frame. gate_center = DEGREE_PER_PIXEL * (gate_data.left_pole + gate_data.right_pole) / 2 # degrees # If both poles are seen, point toward it then go forward. self.gate_seen += 1 self.gate_lost = 0 if abs(gate_center) < STRAIGHT_TOLERANCE: sw3.nav.do(sw3.CompoundRoutine([ sw3.Forward(FORWARD_SPEED), sw3.HoldYaw() ])) else: print "Correcting Yaw", gate_center sw3.nav.do(sw3.CompoundRoutine([ sw3.RelativeYaw(gate_center), sw3.Forward(SLOW_FORWARD_SPEED) ])) # if it has been seen alot, but has been lost, increment the lost counter elif self.gate_seen >= 15: self.gate_lost += 1 # if gate_lost counter has gotten too high, it's definately gone. Move on. if self.gate_lost > 30 or self.mission_timeout <= 0: # tell the user whether the gate was lost or the mission timed out print("Gate lost: %s , timeout: %s" % (self.gate_lost>5, self.mission_timeout <= 0)) # print the timeout if necessary. if self.mission_timeout <= 0: print "Gate Mission Timeout!" # pretty much just a dummy message just to show that we're moving on. print "Heading Locked" # move on to the next state, because we know at this point that # we're pretty much in front of the hedge task and ready to do a 180. self.pass_with_style() # terminate the mission. move on. print "hi" self.finish_mission() 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
def state_approach(self, buoy_to_bump, buoys): print("approach state: {} buoys detected".format(len(buoys))) # TODO: include depth routine if len(buoys) == 3: buoys.sort(key=lambda x: x.theta) self.tracking_id = buoys[buoy_to_bump].id if self.depth_seen is None: self.depth_seen = sw3.data.depth() # any available buoys represent the one we want to hit, track it track_buoy = None for buoy in buoys: if buoy.id == self.tracking_id: track_buoy = buoy # if no hits on finding the target buoy, go back and try again. # mission will time out if this happens too much. if not track_buoy: return # print "State: BuoyBump dist: ",track_buoy.r, " x angle from current: ",track_buoy.theta, " y angle from current: ", track_buoy.phi # various buoy related routines yaw_routine = sw3.RelativeYaw(track_buoy.theta) forward_routine = sw3.Forward(FORWARD_SPEED) stop_routine = sw3.Forward(0,0) backup_routine = sw3.Forward(BACKWARD_SPEED) reset_routine = sw3.SetYaw(self.reference_angle) #change Depth track_depth_angle = (track_buoy.phi) centered = False if abs(track_depth_angle) > DEPTH_THRESHOLD: if (track_depth_angle > 0): depth_routine = sw3.RelativeDepth(-DEPTH_UNIT) if (track_depth_angle < 0): depth_routine = sw3.RelativeDepth(DEPTH_UNIT) else: depth_routine = sw3.NullRoutine() centered = True # check if yaw is centered if abs(track_buoy.theta) <= CENTER_THRESHOLD: centered = True*centered # if yaw and depth are centered, advance to the next state! if centered: sw3.nav.do(sw3.CompoundRoutine( forward_routine, yaw_routine, depth_routine )) ''' if abs(track_buoy.r) <= DIST_THRESHOLD: self.delete_timer("Approach_Timeout") self.next_state() ''' self.next_state() # otherwise, stop and adjust yaw else: sw3.nav.do(sw3.CompoundRoutine( stop_routine, yaw_routine ))
def pickup(self): sw3.nav.do( sw3.CompoundRoutine(sw3.SetDepth(9), sw3.Forward(0, 0), sw3.SetDepth(0))) self.nextState()