Motion planning implemented, TODO - comments
Added comments and more dificult obsticles