Hallo,
fang bitte nicht an alles in globale Variablen zuverfrachten. Das kann zu unklaren Zuständen führen wenn man den Überblick verliert. Zudem sind "gekappselte" Funktionen einfacher portierbar.
Geschwindigkeitsmessung. Ganz einfach und nicht alles in die update Funktion werfen. Schreib eine neue Funktion die in loop regelmäßig aufgerufen wird. Der Zählerstand ist global verfügbar. Guck dir serieller Monitor an. Der wird aller 500ms aufgerufen. Das heißt alles da drin wird nur aller 500ms abgearbeitet. Das kannste dir meinetwegen kopieren und erhöhst es auf 1s. Dann musste dir noch den alten Zählerstand merken und mit dem neuen vergleichen und Differenz bilden und damit die Geschwindigkeit ausrechnen und wieder den neue Zähler als alten merken. Das passiert dann regelmäßig aller 1s. Du musst ja nicht jede Zähleränderung mitbekommen. Du musst nur wissen was war der Zählerstand vor und nach der 1s.