Working on sonar obstacle avoidance