def sweep_routine(self, approach_angle): return sw3.LoopRoutine( sw3.Forward(BACKWARD_SPEED, BACKUP_TIME), sw3.Forward(0, 2), sw3.SetYaw(self.reference_angle), sw3.SetYaw(approach_angle), )
def state_orienting(self, path_data): current_yaw = sw3.data.imu.yaw() * (pi / 180) % (2 * pi) path_angle = (path_data.theta + current_yaw) % pi sw3.nav.do(sw3.Forward(0)) opposite_angle = (pi + path_angle) % (2 * pi) print "Status: Orienting yaw ", current_yaw, " path_angle ", path_angle, " opposite_angle ", opposite_angle if util.circular_distance(self.reference_angle, opposite_angle) < util.circular_distance( self.reference_angle, path_angle): path_angle = opposite_angle if path_angle > math.pi: path_angle = path_angle - 2 * pi print "Orienting to", (180 / pi) * path_angle routine = sw3.SetYaw((180 / pi) * path_angle, timeout=15) routine.on_done(self.finish_mission) sw3.nav.do(routine) self.state = 'done' degree = path_data.theta * (180 / pi) # if degree <= MIN_ANGLE_THRESHOLD or degree >= MAX_ANGLE_THRESHOLD: '''
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 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 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 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 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 sweep_routine2(self): sw3.SequentialRoutine( sw3.Forward(BACKWARD_SPEED, BACKUP_TIME), sw3.Forward(0, 2), sw3.SetYaw(self.reference_angle))
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()