Cherry-picked changes/fixes to flood fill algorithm in order to start testing.
Minor bug fixes and refactoring.  Have robot finding middle of the maze using simulated maze/sensors.