Fix track stepping

1 files changed, 4 insertions, 2 deletions

diff --git a/src/pico1541.c b/src/pico1541.c
index 5c16e0a..f76d8c3 100644
--- 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;