Added compatibility with Arduino Mega

Hi,
I made your code compatible with Arduino Mega (tested on 1280 but should work as fine on 2560).
Thanks for the code.
Sofian