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():
print "FuriousRobot test script"
print "========================"
# Initialize cmd-line params to default values
port = None
scan_ports = ["/dev/ttyACM%i" % i for i in xrange(9, -1, -1)]
config_file = None
# Process command line arguments
args = sys.argv[1:]
while len(args) > 0:
flag = args.pop(0)
if flag == "-p":
# port
port = args.pop(0)
elif flag == "-f":
# config file
config_file = args.pop(0)
# Instantiate and configure the robot
robot = FuriousRobot()
# To where (if anywhere) should the robot write debug messages?
robot.set_debugger(sys.stdout)
if config_file:
print "Reading robot configuration from %s" % config_file
configure_robot(robot, config_file)
else:
print "Manually configuring the robot."
robot.add_servo("steering", 6, 200, 250, 300)
robot.add_servo("throttle", 7, 200, 250, 300)
robot.add_sonars(11, 12, 13)
# Configure the port that the Furious board is connected to.
if not port:
print >>sys.stderr, "No port specified. Probing..."
# If no port is specified at the command line, then scan typical ports.
port = robot.probe(scan_ports)
if not port:
print >>sys.stderr, "Unable to find a valid Furious device. Exiting..."
exit(1)
print "Connecting to port %s." % port
robot.connect(port)
# Initialize servo values.
steer = 0
throt = 0
# Run for a fixed amount of time.
for i in range(20):
robot.refresh()
print "Distance: %.2f" % robot.get_odometer()
# Print the voltage for each analog input and the distance
# that voltage would correspond to if interpreted as an
# infrared rangefinder.
for i in range(6):
print "Analog %i: %.2f volts IR: %.2f meters" % (
i, robot.get_analog(i), robot.get_ir(i) )
print "Sonars: %.2f %.2f %.2f" % (
robot.get_sonar(11),
robot.get_sonar(12),
robot.get_sonar(13) )
robot.set_servo("steering", steer)
robot.set_servo("throttle", throt)
# Change the servo values some arbitrary amount.
steer += 5
throt += 1
time.sleep(1.0/20.0)
print " Exiting..."
robot.stop()
robot.disconnect()
