def main():
print "Starting up..."
# Assign signal handlers
signal.signal(signal.SIGINT, interrupt_handler)
## Load options
cfgfile = "robot.cfg.example"
throttle = THROTTLE
port = None
args = sys.argv[1:]
while len(args) > 0:
opt = args.pop(0)
if opt == "-c":
cfgfile = args.pop(0)
elif opt == "-t":
throttle = clamp( int(args.pop(0)), -100, 100)
elif opt == "-p":
port = args.pop(0)
if not port:
print >>sys.stderr, "Missing required param PORT"
print >>sys.stderr, __doc__ % sys.argv[0]
exit(1)
## Initialization
robot = FuriousRobot()
configure_robot(robot, cfgfile)
robot.connect(port)
robot.stop()
time.sleep(1)
robot.stop()
dist_r = dist_l = dist_f = MAX_RANGE
pid = PidControl(kp=KP, ki=KI, kd=KD)
escape = 0
print "odom, dist_l, dist_r, steer, dist_f, throt, log, mot"
global running
while running:
ri = robot.get_ir(IR_RIGHT)
le = robot.get_ir(IR_LEFT)
fr = robot.get_ir(IR_FRONT)
if ri > MIN_RANGE and ri < MAX_RANGE:
dist_r = ri
if le > MIN_RANGE and le < MAX_RANGE:
dist_l = le
if fr > MIN_RANGE and fr < MAX_RANGE:
dist_f = fr
odom = robot.get_odometer()
steer = int(clamp(pid.getOutput(dist_r, dist_l), -100, 100))
throt = throttle
if escape > 5:
throt = 0
steer = 0
escape -= 1
print "Pre-escape %i" % escape
elif escape > 0:
throt = ESC_THROTTLE
steer = 0
escape -= 1
print "Escape %i" % escape
elif dist_f < ESC_DIST:
escape = 8
throt = 0
steer = 0
print "Danger! Danger!"
else:
print "%.2f, %.2f, %.2f, %4i, %.2f, %4i, %3i%%, %3i%%" % (
odom,
dist_l, dist_r, steer,
dist_f, throt,
robot.get_logic_battery(), robot.get_motor_battery() )
robot.set_servo("steering", steer)
robot.set_servo("throttle", throt)
robot.refresh()
time.sleep(1.0 / LOOP_FREQ)
## Shutdown
print "Shutting down normally..."
robot.stop()
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()
