Fix track stepping

diff --git a/src/pico1541.c b/src/pico1541.c
index 5c16e0af5b131ab29ff0422f47fddb3f42dd618d..f76d8c3ce9a57ab2b066e6a7671422cf096b28da 100644 (file)
--- a/src/pico1541.c
+++ b/src/pico1541.c
@@ -69,9 +69,11 @@ void via_pb_write(via_t *via, uint8_t cycle) {
        } else if (via == &via2) {
                if ((via->ddrb & 3) == 3) {
                        int8_t step = via->orb & 3;
-                       if (step - track_step > 0) {
+                       int8_t r = (step - track_step) % 4;
+                       uint8_t mod = r < 0 ? r + 4 : r;
+                       if (mod == 1) {
                                drive_step_up(&drive);
-                       } else if (step - track_step < 0) {
+                       } else if (mod == 3) {
                                drive_step_down(&drive);
                        }
                        track_step = step;