Versuche das ganze mit Interrupts und StateEvent zu realisieren. Das heisst du reagierst nur auf spezielle Zeitkritische Eingaben z.B. Servo-Steuerung. Für Display und so hast du ja danach noch genug Zeit.
Mach besser zuviele Funktionen als zuwenige! Spaghetticode wird sofort unübersichtlich und unpraktisch!