Refactor of autonomous and bugfixes

Removed old deprecated test code.
Removed unused variables.
Fixed a small bug in the pathfinding for checking if a square was
covering a wall