Drive, LED, Fixes

Cleaned up autonomous code, refactored the drive code with rotate, and
added the LED code