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