Overhauled. Cleaner, but yet untested on hardware
Merge pull request #1 from MikeRixWolfe/revision

Overhauled. Cleaner, but yet untested on hardware