Updated for better performance with Leonardo. And fixed compile bug under Arduino 1.5.4