diff --git a/dda.c b/dda.c
index 952c86b..846af3a 100644
--- a/dda.c
+++ b/dda.c
@@ -432,11 +432,6 @@ void dda_step(DDA *dda) {
 			x_step();
 			did_step = 1;
 			dda->x_steps--;
-	// 				if (dda->x_direction)
-	// 					current_position.X++;
-	// 				else
-	// 					current_position.X--;
-
 			dda->x_counter += dda->total_steps;
 		}
 	}
@@ -448,11 +443,6 @@ void dda_step(DDA *dda) {
 			y_step();
 			did_step = 1;
 			dda->y_steps--;
-	// 				if (dda->y_direction)
-	// 					current_position.Y++;
-	// 				else
-	// 					current_position.Y--;
-
 			dda->y_counter += dda->total_steps;
 		}
 	}
@@ -464,11 +454,6 @@ void dda_step(DDA *dda) {
 			z_step();
 			did_step = 1;
 			dda->z_steps--;
-	// 				if (dda->z_direction)
-	// 					current_position.Z++;
-	// 				else
-	// 					current_position.Z--;
-
 			dda->z_counter += dda->total_steps;
 		}
 	}
@@ -479,11 +464,6 @@ void dda_step(DDA *dda) {
 			e_step();
 			did_step = 1;
 			dda->e_steps--;
-	// 				if (dda->e_direction)
-	// 					current_position.E++;
-	// 				else
-	// 					current_position.E--;
-
 			dda->e_counter += dda->total_steps;
 		}
 	}
@@ -565,6 +545,8 @@ void dda_step(DDA *dda) {
 		z_disable();
 	}
 
+	cli();
+
 	setTimer(dda->c >> 8);
 
 	// turn off step outputs, hopefully they've been on long enough by now to register with the drivers
diff --git a/dda_queue.c b/dda_queue.c
index 5bc87be..8a37ae1 100644
--- a/dda_queue.c
+++ b/dda_queue.c
@@ -28,12 +28,20 @@ DDA movebuffer[MOVEBUFFER_SIZE] __attribute__ ((__section__ (".bss")));
 
 /// check if the queue is completely full
 uint8_t queue_full() {
-	return (((mb_tail - mb_head - 1) & (MOVEBUFFER_SIZE - 1)) == 0)?255:0;
+	uint8_t sreg = SREG, r;
+	cli();
+	r = (((mb_tail - mb_head - 1) & (MOVEBUFFER_SIZE - 1)) == 0)?255:0;
+	SREG = sreg;
+	return r;
 }
 
 /// check if the queue is completely empty
 uint8_t queue_empty() {
-	return ((mb_tail == mb_head) && (movebuffer[mb_tail].live == 0))?255:0;
+	uint8_t sreg = SREG, r;
+	cli();
+	r = ((mb_tail == mb_head) && (movebuffer[mb_tail].live == 0))?255:0;
+	SREG = sreg;
+	return r;
 }
 
 // -------------------------------------------------------
@@ -95,9 +103,15 @@ void enqueue(TARGET *t) {
 
 	mb_head = h;
 
+	uint8_t sreg = SREG;
+	cli();
 	// fire up in case we're not running yet
-	if (movebuffer[mb_tail].live == 0)
+	if (movebuffer[mb_tail].live == 0) {
+		SREG = sreg;
 		next_move();
+	}
+	else
+		SREG = sreg;
 }
 
 /// go to the next move.
@@ -144,6 +158,9 @@ void queue_flush() {
 	mb_tail = mb_head;
 	movebuffer[mb_head].live = 0;
 
+	// disable timer
+	setTimer(0);
+
 	// restore interrupt flag
 	SREG = sreg;
 }