Cleanup & Fix for Mega2560