Finished all climbing steps as command based robot