Major implementations and fixes.

Wrote framework code for sweeped coverage and simple coverage. Fixed
path planning bugs.