major refactoring

repairing servo off (switch off after time out, not between kisses),
refactoring ultrasound, ...