Hallo Felix H.,

bevor ich verstehen konnte welche Aufgabe die Unterprogramme Wait_one und Wait_zero haben, habe ich mich gefragt, weshalb Du in den Unterprogrammen eine Do-Loop-Schleife eingebaut hast - und noch dazu eine ohne Bedingung (im Sinne von "Loop until X = ...").
Ich könnte mir vorstellen, dass der Controller dann beim ersten Ausführen des Unterprogramms genau dort hängenbleibt, weil er nie den Return-Befehl erreicht.

Ob das eine Erklärung für den beobachteten Fehler ist, kann ich auf die Schnelle nicht sagen, weil sich mir das ganze Programm noch nicht vollständig erschließt, aber probier´s doch einfach mal ohne Do-Loop in den Subroutinen.