Team 862, over the summer extracted and cleaned up our drive train code to get ready for this season. The code is still a work in progress, but it includes support for two speed, auto shifting, recording and playback of session, extensive logging and is relatively clean.
We have not tested any of the code on a roboRIO, but there is nothing odd in there that should cause a problem. There are certainly a couple bugs waiting, but we will patch them as we find them, until we make a private branch and start adding game specific stuff in January.
Hope it helps someone, and if you find any bugs let us know :-)
https://github.com/frc-862/lightning-base