Code cleanup ready for servo pen lift function