Serial.print Program Lag

You have to replace all of your Serial.print(foo); debug statements with PRINTS(foo)' and etc.

I like

 static const DEBUG = 0;
#define debugPrint(...) do (if (DEBUG) Serial.print(__VA_ARGS__) while (0)
#define debugPrintln(...) do (if (DEBUG) Serial.println(__VA_ARGS__) while (0)

This way, if you make DEBUG a static const as above the compiler optimization will give you either a simple Serial.print, or nothing, but you also have the option of making DEBUG a variable that you can change at runtime.