def run(): # power_level 25 doesn't go, 30 does. try: delay = Config.getfloat('power', 'delay') Logger.info("start: delay %.3f", delay) with control.pololu(timeout=1) as ctl: if not os.exists(Input_filename): open(Input_filename, 'w').close() with open(Input_filename) as input: line = None # read to EOF: for line in input.readline(): pass # wait for first line: while not line: time.sleep(0.2) line = input.readline() while True: if line: power_level1, power_level2 = \ (int(x) for x in line.split()) Logger.info("power_level1 %d, power_level2 %d", power_level1, power_level2) time.sleep(delay) ctl.set_power(power_level1) time.sleep(delay) ctl.set_power(power_level2) line = input.readline() except Exception, e: print >> sys.stderr, "got exception" Logger.exception("%s: %s", e.__class__.__name__, e) raise
def run(duration=None, power_level1=None, power_level2=None, fudge=None): # power_level 25 doesn't go, 30 does. if duration is None: duration = Config.getint('follow', 'duration') if power_level1 is None: power_level1 = Config.getint('follow', 'power_level1') if power_level2 is None: power_level2 = Config.getint('follow', 'power_level2') if fudge is None: fudge = Config.getfloat('follow', 'steering_fudge') try: Logger.info("start: duration %d, power_level1 %d, power_level2 %d, " \ "steering fudge %.1f", duration, power_level1, power_level2, fudge) with control.pololu(timeout=1) as ctl: cp = compass(ctl) with gps() as g: # wait for first heading: while g.read() is None: pass ctl.set_range_finder(0) # look straight ahead # preload compass values to get averaging going... for i in range(cp.maxlen): cp.read() time.sleep(0.1) # Go! try: Logger.info("starting vehicle!") power_levels = power_level1, power_level2 ctl.set_power(power_level1) time.sleep(0.5) ctl.set_power(power_level2) start = time.time() iterations = 0 did_wall = False #print "obstacle_dist actual_heading correction " \ # "processing_time" while time.time() - start < duration: start_tenth = time.time() ctl.set_power(power_levels[iterations & 1]) obstacle_dist = ctl.read_distance() # close obstacle detection #FIX if obstacle_dist < 500: if obstacle_dist < 300: if did_wall: ctl.set_power(0) Logger.info("2nd obstacle encountered " "at distance %d", obstacle_dist) break print >> sys.stderr, "going to wall_following" Logger.info("obstacle encountered at distance %d", obstacle_dist) wall_follow(cp, ctl, power_levels, start, duration, g, 1, 300) did_wall = True actual_heading = cp.read() target_heading = g.read() if target_heading == 1000.0: Logger.info("got STOP from gps_nav.py") break # positive is right turn correction = target_heading - actual_heading # correct for small differences around +/-180: if abs(correction) > 180.0: if correction > 0.0: correction -= 360.0 else: correction += 360.0 ctl.set_steering(correction * fudge) processing_time = time.time() - start_tenth Logger.debug("obstacle_dist %.1f, actual_heading %.1f, " "correction %.1f, processing_time %.3f", obstacle_dist, actual_heading, correction, processing_time) iterations += 1 time_left = 0.1 - processing_time if time_left > 0.0: time.sleep(time_left) total_time = time.time() - start Logger.info("done: total time %.2f, iterations %d, " "msec/iteration %.0f", total_time, iterations, (total_time / iterations) * 1000.0) finally: # Stop! Logger.info("done: stopping vehicle") ctl.set_power(0) time.sleep(1) ctl.set_power(0) except Exception, e: print >> sys.stderr, "got exception" Logger.exception("%s: %s", e.__class__.__name__, e) raise
def run(duration=None, power_level1=None, power_level2=None, fudge=None): # power_level 25 doesn't go, 30 does. if duration is None: duration = Config.getint('follow', 'duration') if power_level1 is None: power_level1 = Config.getint('follow', 'power_level1') if power_level2 is None: power_level2 = Config.getint('follow', 'power_level2') if fudge is None: fudge = Config.getfloat('follow', 'steering_fudge') try: Logger.info("start: duration %d, power_level1 %d, power_level2 %d, " \ "steering fudge %.1f", duration, power_level1, power_level2, fudge) with control.pololu(timeout=1) as ctl: cp = compass(ctl) with gps() as g: # wait for first heading: while g.read() is None: pass ctl.set_range_finder(0) # look straight ahead # preload compass values to get averaging going... for i in range(cp.maxlen): cp.read() time.sleep(0.1) # Go! try: Logger.info("starting vehicle!") power_levels = power_level1, power_level2 ctl.set_power(power_level1) time.sleep(0.5) ctl.set_power(power_level2) start = time.time() iterations = 0 did_wall = False #print "obstacle_dist actual_heading correction " \ # "processing_time" while time.time() - start < duration: start_tenth = time.time() ctl.set_power(power_levels[iterations & 1]) obstacle_dist = ctl.read_distance() # close obstacle detection #FIX if obstacle_dist < 500: if obstacle_dist < 300: if did_wall: ctl.set_power(0) Logger.info( "2nd obstacle encountered " "at distance %d", obstacle_dist) break print >> sys.stderr, "going to wall_following" Logger.info("obstacle encountered at distance %d", obstacle_dist) wall_follow(cp, ctl, power_levels, start, duration, g, 1, 300) did_wall = True actual_heading = cp.read() target_heading = g.read() if target_heading == 1000.0: Logger.info("got STOP from gps_nav.py") break # positive is right turn correction = target_heading - actual_heading # correct for small differences around +/-180: if abs(correction) > 180.0: if correction > 0.0: correction -= 360.0 else: correction += 360.0 ctl.set_steering(correction * fudge) processing_time = time.time() - start_tenth Logger.debug( "obstacle_dist %.1f, actual_heading %.1f, " "correction %.1f, processing_time %.3f", obstacle_dist, actual_heading, correction, processing_time) iterations += 1 time_left = 0.1 - processing_time if time_left > 0.0: time.sleep(time_left) total_time = time.time() - start Logger.info( "done: total time %.2f, iterations %d, " "msec/iteration %.0f", total_time, iterations, (total_time / iterations) * 1000.0) finally: # Stop! Logger.info("done: stopping vehicle") ctl.set_power(0) time.sleep(1) ctl.set_power(0) except Exception, e: print >> sys.stderr, "got exception" Logger.exception("%s: %s", e.__class__.__name__, e) raise