diff --git a/timer.c b/timer.c
index dd2bbbf..44556a9 100644
--- a/timer.c
+++ b/timer.c
@@ -100,9 +100,14 @@ void setTimer(uint32_t delay)
 			timer1_compa_isr();
 		}
 		else if (next_step_time <= TICK_TIME) {
+			// next step occurs before overflow, set comparator here
 			OCR1A = next_step_time & 0xFFFF;
 			TIMSK1 |= MASK(OCIE1A);
 		}
+		else {
+			// adjust next_step_time so overflow interrupt sets the correct timeout
+			next_step_time -= TICK_TIME;
+		}
 	}
 	else {
 		next_step_time = 0;