Code ist soweit i.O., auch wenn wait nicht präzise ist (Da besser Timer nehmen). Entweder liegts daran oder an deiner Hardware, wie ist der Mega beschaltet?