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()