Make work on Teensy 3.1 (probably 3 as well)

Do like I did for Arduino Due and simply use digitalWrites and the like
instead of hitting registers as processors are fast enough