Add Arduino code
Cleaner testing