Generell würde ich das so machen:

Du wartest scheinbar zwischen den einzelnen Steps immer 1600µs.
Daher würde ich einen Timer so aufsetzen, dass er alle 1600µs überläuft. In der ISR fragst du dann die Eingänge ab (so wie in deinem Code) und erzeugst einen Schritt für jede Richtung, die noch nicht fertig ist (Status Variable noch nicht die 1200, 8600 usw. erreicht). Dann erhöhst du eine Status Variable der jeweiligen Richtung. Also keine komplette Schleife, wie bei dir.
Bei 1MHz Takt sind dann die 10µs um und die setzt die Ausgänge in der gleichen Reihenfolge wieder zurück.
Die 10µs sind bei 1µs Systemtakt aber ziemlich schlecht einzuhalten. Würde ich schon eher 8MHz nehmen.
Sind die abgefragten Pins eigentlich Endschalter oder wodurch werden immer neue Loops verhindert?

Gruß

Rolf