Changed waypoints and control to allow step tests. This is the body Y axis version.