The wpi is a good place to start...I would learn from examples. My team has our live robot code uploaded and documented on
github here
If you have any questions feel free to pm me anytime and ill get back with answers. This is not my code as I have other responsibilities for the team but i oversee most of it still.