Add teensy 3.x support, fix example bug, cleanup and reorganize some blocks of code