Added arguments to findpath and setgoal