Code update

Put sonar ping into SonarPing() object. Reduced the distance the robot travels forward before doing another ping.

This has been tested and improves object detection by quite a lot.