fixes for obstacle avoidance. ready to test on robot