bin nicht sicher ob ich Dein Anliegen verstanden habe. Eine Möglichkeit für den Grundaufbau Deines Programmes ist folgende Struktur:

1. Abfrage des gewünschten Modus und Sprung zur entsprechenden Hauptroutine

2. Hauptroutinen
2.1. Hautproutine Modi 1
2.2. Hauptroutine Modi 2
2.3. Hauptroutine Modi 3

3. Werkzeuge
3.1. z.B. drehe nach links
3.2. z.B. drehe nach rechts
3.3. ...

Den Sprung zur Modiabfrage kann man z.B. über den IRQ machen.

Ist es das, wonach Du gefragt hast?