Code Structure, Organization, Documentation

Added more documentation.
Organized the code a little more.
Changed the code structure, separated autonomous code from operator code
and test code.
New code structure needs to be tested.

This will be the starting point for all future branches