Added wait and stop to autonomous