Added logic loop and path planner with no collision. Move class structure (see class diagram).