Cleaned code and added more autonmous commands

John did this on the busride to Palm Meadow