more properly set up simulation step and set path functions; able to perform first waypoint