elif opt == "-kd":
kd = int(args.pop(0))
else:
print "Unknown argument %s" % opt
exit(1)
# Start things up
client = tbrclient()
print "connecting... "
client.initialize(1225, "127.0.0.1")
ai = SpeedControl(kp=kp, ki=ki, kd=kd)
ai.setObserver(sys.stdout)
if client.ready():
try:
while True:
cur_dist = client.getOdometerDistance()
throt = ai.calculateThrottle(target_vel, cur_dist)
client.setThrottle(throt)
time.sleep(LOOP_PERIOD)
except KeyboardInterrupt:
print " Shutting down..."
client.setThrottle(0)
else:
print "Error: unable to connect to tbrprobe."
client.setThrottle(0)
client.finalize()
def main():
global running, client
print "Starting up..."
# Assign signal handlers
signal.signal(signal.SIGINT, interrupt_handler)
test = False
args = sys.argv[1:]
mapfile = MAPFILE
while len(args) > 0:
opt = args.pop(0)
if opt == "test":
print "===== TEST MODE ON ===="
test = True
else:
mapfile = opt
map = maps.loadSegmentMap(mapfile)
seg = 0
uptime = 0.0
bot = SonarBot(pv=PV, mv=MV, fmix=FMIX)
steer_pid = PidControl(KP, KI)
spd_ctrl = SpeedControl(kp=50, ki=8)
escape = False
pthrottle = 0
prev_l = prev_r = 0.0
client.initialize(1225, "127.0.0.1")
time.sleep(1)
running = client.ready()
print "mode, segment, distance, left, front, right, dturn, aturn, throttle"
while running:
seg_start = cur_dist = client.getOdometerDistance()
ml = map[seg].length
mt = map[seg].turn
prev_turn = 0
while cur_dist - seg_start < ml:
# Initialize variables
turn = mt # modify the map turn based on sensors
(f, l, r) = bot.getDangers(client)
if test:
print "testing ",
throttle = THROTTLE
# Warm up the sonar filters
elif uptime < 2.0:
print "warmup ",
turn = 0
throttle = 0
# If in the middle of escaping, continue
elif escape:
print "escaping ",
if prev_turn == 0:
turn = 0
else:
turn = 100 * prev_turn / abs(prev_turn)
throttle = ESCAPE_THROTTLE
if f < SAFE:
escape = False
# If cornered, back up.
elif f > ESCAPE:
print "escape! ",
escape = True
throttle = 0
# Otherwise proceed normally
else:
print "driving ",
delt_r = r - prev_r
delt_l = l - prev_l
turn += steer_pid.getOutput(0, delt_r - delt_l)
throttle = spd_ctrl.calculateThrottle(SPEED, cur_dist)
correction = DIST_CORR * (abs(mt) - abs(turn) )
seg_start += correction
turn = int( clamp(turn, -100, 100) )
client.setSteering(turn)
throttle = (TMIX * throttle) + ((1 - TMIX) * pthrottle)
throttle = int( clamp( throttle, -100, 100 ) )
# check the kill switch
if client.getKillSwitchVal() < KILL_OFF:
throttle = 0
client.setThrottle(throttle)
# Prepare for next iteration
print ", %i, %.2f, %.2f, %.2f, %.2f, %i, %i, %i" % (
seg, cur_dist, l, f, r, mt, turn, throttle)
time.sleep(LOOP_PERIOD)
uptime += LOOP_PERIOD
cur_dist = client.getOdometerDistance()
prev_turn = turn
pthrottle = throttle
seg += 1
# Begin next lap
if seg >= len(map):
seg = 0
print "Exiting normally."
shutdown()