Millis() code needs improvement