Implemented checking x,y,z servo states and resetting after clear