Massive refactoring and code deletion.
Deleted all code that will not be needed on current robot.
Renamed and refactored to clarify naming.