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
