def findpath(self): if self.turn_count % 2: sw3.nav.do(sw3.RelativeYaw(-90)) else: sw3.nav.do(sw3.RelativeYaw(90)) sw3.nav.do(sw3.Forward(.1, 2)) self.finish_mission()
def pass_with_style(self): """the code for turning the robot, and passing through the hedge with style.""" turn_routine = sw3.RelativeYaw(180) stop_routine = sw3.Forward(0, 0.1) backup_routine = sw3.Forward(BACKUP_SPEED, BACKUP_TIME) # stop sw3.nav.do(stop_routine) time.sleep(1) # 180 sw3.nav.do( stop_routine, turn_routine,) # do a 180 time.sleep(10) sw3.nav.do(backup_routine) # backup time.sleep(BACKUP_TIME) time.sleep(1) sw3.nav.do(stop_routine) # stop time.sleep(1) print "doing the 2nd 180" sw3.nav.do(sw3.RelativeYaw(180)) # do another 180 time.sleep(10) print "done"
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 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 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): 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 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 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 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 processFrame(self, frame): print "turn state" path = vision.ProcessFrame(frame) if path.found: print "path found" self.pathLost.restart() """ finding out where the start of the path is. This is the path end point closest to the center of the camera """ #pt1, pt2 = path.endPoints() pts = [[path.p1x, path.p1y], [path.p2x, path.p2y]] self.startPt, self.endPt = sortPoints(self.startPt, pts) center = (path.cx, path.cy) angle = turnParallelTo(center, self.endPt) print "Angle: %d" % angle if abs(angle) <= MAXANGLEDIF: sw3.RelativeYaw(0).start() return OnwardState() elif not self.pathLost.timeLeft(): """if the path has been lost for too long go to path lost state""" return OnwardState() print "ret found" return self
def processFrame(self, frame): print "found state" path = vision.ProcessFrame(frame) if path.found: print "path found" self.pathLost.restart() """ Turning to face the end point of the path. """ #pt1, pt2 = path.endPoints() pts = [[path.p1x, path.p1y], [path.p2x, path.p2y]] self.startPt, self.endPt = sortPoints(self.startPt, pts) center = (path.cx, path.cy) angle = turnParallelTo(center, self.endPt) print "Angle: %d" % angle if abs(angle) <= MAXANGLEDIF: sw3.RelativeYaw(0).start() return ToEndState(self.startPt, self.endPt) elif not self.pathLost.timeLeft(): """if the path has been lost for too long go to path lost state""" return PathLostState() print "ret found" return self
def processFrame(self, frame): gate = vision.ProcessFrame(frame) print gate.found if gate.found: frame = gate.draw(frame) """ finding out how many pixels from the center the gate is the center obj of the gate is the number or pixels over the gate is. Subtracting the middle pixel index from it returns a pos value if the gate is to left and pos value if the gate is to the right """ _, w, _ = frame.shape center = w / 2.0 - gate.cp print("got center %d" % center) self.centers.insert(0, center) centers = 0 for i in self.centers: centers += i center = float(centers) / len(self.centers) print(center) sw3.RelativeYaw(center / PIXTODEG).start() cv2.imshow(self.name, frame) return self.runtime > (time.time() - self.time)
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 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 turnToAngle(angle): angle *= 180.0 / math.pi print "NEW ANGLE", angle #gets the angle it needs to turn by if angle < 0: angleDif = abs(angle) - 90 elif angle < 90: angleDif = -90 - angle else: angleDif = 270 - angle sw3.RelativeYaw(angleDif).start() return angleDif
def processFrame(self, frame): yaw = sw.var.get("Acoustics.Yaw") pitch = abs(sw.var.get("Acoustics.Pitch")) print("Yaw: %4.2f, Pitch %4.2f" % (yaw, pitch)) if yaw != self.yaw or pitch != self.pitch: self.yaw = yaw self.pitch = pitch if pitch >= PITCHCUT: sw3.RelativeYaw(yaw).start() else: return PitchState(pitch) return self
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 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 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 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 state_centering(self, x, y): position_rho = math.sqrt( x**2 + y**2) # hypotenuse-distance from frame center to path center position_phi = math.atan2(x, y) * ( 180 / pi) # angle of center of path from current position print "State:Centering dist ", position_rho, " angle from current ", position_phi sw3.nav.do(sw3.Forward(0)) yaw_routine = sw3.RelativeYaw(position_phi) forward_routine = sw3.Forward(FORWARD_SPEED, CENTER_TIME) sw3.nav.do(yaw_routine) yaw_routine.on_done(lambda x: sw3.nav.do(forward_routine)) if position_rho <= CENTER_THRESHOLD: self.state = "orienting"
def processFrame(self, frame): print "found state" path = vision.ProcessFrame(frame) if path.found: print "path found" self.pathLost.restart() """ finding out how many pixels from the center the gate is the center obj of the gate is the number or pixels over the gate is. Subtracting the middle pixel index from it returns a pos value if the gate is to left and pos value if the gate is to the right """ print("got direction %d" % path.orientation) sw3.RelativeYaw(path.orientation).start() elif not self.pathLost.timeLeft(): """if the gate has been lost for too long go to gate lost state""" return PathLostState() print "ret found" return self
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 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 processFrame(self, frame): print "found state" gate = vision.ProcessFrame(frame) if gate.found: print "gate found" self.gateLost.restart() """ finding out how many pixels from the center the gate is the center obj of the gate is the number or pixels over the gate is. Subtracting the middle pixel index from it returns a pos value if the gate is to left and pos value if the gate is to the right """ _, w, _ = frame.shape center = w / 2.0 - gate.cp print("got center %d" % center) self.centers.insert(0, center) centers = 0 for i in self.centers: centers += i center = float(centers) / len(self.centers) print(center) print self.centers if len(self.centers) > 10: self.centers.pop() print self.centers #if less than set difference ignore it center = center if center > SIGPIX else 0 sw3.RelativeYaw(center / PIXTODEG).start() elif not self.gateLost.timeLeft(): """if the gate has been lost for too long go to gate lost state""" return GateLostState() print "ret found" return self
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): 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 ))
import vision import missions #import acoustics from mission_controller import MissionController # 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.GateMission, missions.PathMission, sw3.nav.do(sw3.RelativeYaw(-5)), missions.PathMission, missions.ReverseHedgeMission, missions.HedgeMission, missions.NewBuoyMission, missions.PathMission, #missions.PathMission, #missions.PathMission, #missions.AcousticsMission, #missions.HedgeMission, #(missions.PathMission, True, 1), #sw3.Forward(.5, 1), #missions.NewBinsMission, #(missions.PathMission, True, 1), #missions.HedgeMission, #missions.FakePizzaMission
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()
from seawolf/mission_control import sw3 sw3.nav.do(sw3.RelativeYaw(10))