def state_disconnect(self):
sw3.nav.do(
sw3.SequentialRoutine(
sw3.Forward(-0.5, 3),
sw3.NullRoutine(),
))
self.wait_stop_mark = 1
self.next_state()
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_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 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 kill(self):
self.process_manager.kill()
sw3.nav.do(sw3.NullRoutine())