Extend motortest to all motors.
Format code.
Working interrupt driven ultrasonic sensor