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