Obstacle avoidance

Fixes the sonars. For the scrimmage!