Reworked the arduino code. Still in progress.