Also, du nimmst einen Timer her und stellst ihn so ein, daß er z.B. im 1ms Takt einen Interrupt erzeugt. In der Timer-ISR zählst du dann im einfachsten Fall mal eine Variable um 1 hoch.

Damit hast du schonmal alles was du brauchst um innerhalb deines Programms Zeiten zu messen, denn du brauchst dir ja nur den Startzeitpunkt merken und erhälst die vergangene Zeit dann einfach durch Subtraktion des Startzeitpunkts von der aktuellen Zeit.

Dadurch wiederum ist es sehr leicht Wartezeiten in Funktionen einzubauen, welche zyklisch aufgerufen werden. Das habe ich ja auch schon beispielhaft in der mini Statemachine dargestellt.