This code is more "advanced" than the one used in the video. Basically every time the sonar servo completes a sweep, the angle for the direction with the maximum distance is determined, and the robot tries to turn in that direction. The direction with the minimum distance is also determined, and the rover tries to turn away from that direction.
-Z-