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()