Some refactoring and better Teensy3.x cooperation