Hello,
I had something similar in the past where the solution was to not to call millis() too many times. But store the value of millis() in a temporary variable each cycle. Second thing you could do is be careful with too many Serial.println() as this might cause some buffer overruns or overload on code sections where interrupts are blocked. Other thing that can be usefull: add delay(10) to your code so the processor has some room to schedule saved interrupt tasks.
Another approach that is much clearer is the use of a timer object instead of the millis().
see Use multiple millis - Using Arduino / Programming Questions - Arduino Forum