Some Pathplanning added