Added (incomplete) tests for distance sensor and servo motor, minor refactoring