Added some commented out to obstacle avoidance to help visualize what is happenning. (Useful for debugging).