Check out Kevin Watson's code here
http://kevin.org/frc/. Everything you need and then some.
Just to add: if I recall the base code has a simple drive mode already. I'm a bit rusty with this code (has it really been four years ago??!) but if you can read C it's doable. Ask here if you have questions. Come to think of it, Kevin still wanders into Chief Delphi when he isn't programming rockets.