Implement sonar, blocks when too close to an obstacle (front and back)