Fixed bugs, finished one obstacle avoidance, started working on multiple obstacle avoidance