Modified code so that it works on the Teensy 3.0 board (should still work on Arduino) and made some minor improvements.