Adding code for firmware. Working with teensy, need to rework hardware part.