Cleaned up some stuff.

Changes were made from files that were tested to work on the robot irl.
These new changes will likely work, but need to be tested.