Modify to use as a library. Tested in Arduino 1.0.5.