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