update wallFollow, add waypoint check