Arduino Mega 2560 - timeout problem - FIXED