diff --git a/timer.c b/timer.c
index 2d4f074..9472aa1 100644
--- a/timer.c
+++ b/timer.c
@@ -29,6 +29,11 @@
 /// time until next step, as output compare register is too small for long step times
 uint32_t	next_step_time;
 
+#ifdef ACCELERATION_TEMPORAL
+/// unwanted extra delays, ideally always zero
+uint32_t	step_extra_time = 0;
+#endif /* ACCELERATION_TEMPORAL */
+
 /// every time our clock fires, we increment this so we know when 10ms has elapsed
 uint8_t						clock_counter_10ms = 0;
 /// keep track of when 250ms has elapsed
@@ -134,28 +139,58 @@ void setTimer(uint32_t delay)
 	// save interrupt flag
 	uint8_t sreg = SREG;
 	uint16_t step_start = 0;
+	#ifdef ACCELERATION_TEMPORAL
+	uint16_t current_time;
+	uint32_t earliest_time, actual_time;
+	#endif /* ACCELERATION_TEMPORAL */
 
 	// re-enable clock interrupt in case we're recovering from emergency stop
 	TIMSK1 |= MASK(OCIE1B);
 
-	// if the delay is too small use a minimum delay so that there is time
-	// to set everything up before the timer expires.
-
-	if (delay < 17 )
-		delay = 17;
+	// An interrupt would make all our timing calculations invalid,
+	// so stop that here.
+	cli();
+	CLI_SEI_BUG_MEMORY_BARRIER();
 
 	// Assume all steps belong to one move. Within one move the delay is
 	// from one step to the next one, which should be more or less the same
 	// as from one step interrupt to the next one. The last step interrupt happend
 	// at OCR1A, so start delay from there.
-	#warning This can't work. If it took some time since the last setTimer() and the delay is very short, the timer will wait almost a full round (3,27 ms).
 	step_start = OCR1A;
 	if (next_step_time == 0) {
 		// new move, take current time as start value
 		step_start = TCNT1;
 	}
-
 	next_step_time = delay;
+
+	#ifdef ACCELERATION_TEMPORAL
+	// 300 = safe number of cpu cycles until the interrupt actually happens
+	current_time = TCNT1;
+	earliest_time = (uint32_t)current_time + 300;
+	if (current_time < step_start) // timer counter did overflow recently
+		earliest_time += 0x00010000;
+	actual_time = (uint32_t)step_start + next_step_time;
+
+	// Setting the interrupt earlier than it can happen obviously doesn't
+	// make sense. To keep the "belongs to one move" idea, add an extra,
+	// remember this extra and compensate the extra if a longer delay comes in.
+	if (earliest_time > actual_time) {
+		step_extra_time += (earliest_time - actual_time);
+		next_step_time = earliest_time - (uint32_t)step_start;
+	}
+	else if (step_extra_time) {
+		if (step_extra_time < actual_time - earliest_time) {
+			next_step_time -= step_extra_time;
+			step_extra_time = 0;
+		}
+		else {
+			step_extra_time -= (actual_time - earliest_time);
+			next_step_time -= (actual_time - earliest_time);
+		}
+	}
+	#endif /* ACCELERATION_TEMPORAL */
+
+	// Now we know how long we actually want to delay, so set the timer.
 	if (next_step_time < 65536) {
 		// set the comparator directly to the next real step
 		OCR1A = (next_step_time + step_start) & 0xFFFF;
@@ -172,11 +207,9 @@ void setTimer(uint32_t delay)
 	}
 
 	// Enable this interrupt, but only do it after disabling
-	// global interrupts. This will cause push any possible
+	// global interrupts (see above). This will cause push any possible
 	// timer1a interrupt to the far side of the return, protecting the 
 	// stack from recursively clobbering memory.
-	cli();
-	CLI_SEI_BUG_MEMORY_BARRIER();
 	TIMSK1 |= MASK(OCIE1A);
 
 	// restore interrupt flag