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_setup_pulldown(self):
# start: buoy has dissappeard b/c we're so close to it
# ###
sw3.nav.do(
sw3.SequentialRoutine(sw3.Forward(-0.7, 1), sw3.Forward(0, 0.1),
sw3.RelativeDepth(-2),
sw3.Forward(0.7, 1 * 1.5),
sw3.Forward(0, 0.1)))
self.wait_stop_mark = 4
self.next_state()
def state_findpath(self):
# TODO: check if the second buoy was center, if so, dont do another approach
riseup_routine = sw3.RelativeDepth(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(depth_goto, riseup_routine, runover_routine))
self.finish_mission()
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 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 bump_timeout(self, approach_angle):
sw3.nav.do(sw3.SequentialRoutine(
sw3.Forward(BACKWARD_SPEED, 10),
self.sweep_routine(approach_angle)
))
self.next_state()
def sweep_routine2(self):
sw3.SequentialRoutine(
sw3.Forward(BACKWARD_SPEED, BACKUP_TIME),
sw3.Forward(0, 2),
sw3.SetYaw(self.reference_angle))
def state_pulldown(self):
sw3.nav.do(
sw3.SequentialRoutine(sw3.Forward(0.7, 0.2), sw3.DepthRate(1, 1),
sw3.RelativeDepth(2), sw3.Forward(0, 0.01)))
self.wait_stop_mark = 3
self.next_state()
