Ported CloseTopServo to cpp