FOD Cleanup - Smart Dashboard!

Field oriented drive still working, just cleaning up some code. Added
Smart dashboard support. Trying to work on a rotate to angle command.