Fixed distance bug
Merge https://github.com/robotics-silver-surfer/surfer-main