Made some changes to support Arduino Mega