Remove all non-Teensy3 code, rearrange functions