In der ISR sollst du auch nicht warten.
Ist warten überhaupt sinnvoll?
Wäre es nicht besser wenn er zurück fährt oder dreht?
Sonst in der ISR ein Flag für warten, eine eigene Funktion für Warten und dort dann das Flag für Beschleunigen setzen.