Completelly changed structure, used Threads. Changed to ArduinoDue