Initial commit of teensy firmware