So, are you using a Delta? A deltabot with a graphical display pushes the limits of a Mega2560, so you may need to reduce DELTA_SEGMENTS_PER_SECOND to get acceptable performance.
If you're not using a delta machine, this is something to look into, as it could mean there is too much processing during the main interrupt.
If you're not using a delta machine, this is something to look into, as it could mean there is too much processing during the main interrupt.