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