Multiple obstacles

Started working on multiple obstacles avoidance, still bugs to be fixed