More documentation in the code. Closer to autonomously mapping. Many
issues fixed.