diff --git a/config.h b/config.h
index 3a82e46..e2811cd 100644
--- a/config.h
+++ b/config.h
@@ -26,9 +26,9 @@
 // Updated default pin-assignments from 0.6 onwards 
 // (see bottom of file for a copy of the old config)
 
-#define STEPPERS_ENABLE_DDR     DDRB
-#define STEPPERS_ENABLE_PORT    PORTB
-#define STEPPERS_ENABLE_BIT         0
+#define STEPPERS_DISABLE_DDR     DDRB
+#define STEPPERS_DISABLE_PORT    PORTB
+#define STEPPERS_DISABLE_BIT         0
 
 #define STEPPING_DDR       DDRD
 #define STEPPING_PORT      PORTD
@@ -61,9 +61,9 @@
 
 // Pin-assignments from Grbl 0.5
 
-// #define STEPPERS_ENABLE_DDR     DDRD
-// #define STEPPERS_ENABLE_PORT    PORTD
-// #define STEPPERS_ENABLE_BIT         2
+// #define STEPPERS_DISABLE_DDR     DDRD
+// #define STEPPERS_DISABLE_PORT    PORTD
+// #define STEPPERS_DISABLE_BIT         2
 // 
 // #define STEPPING_DDR       DDRC
 // #define STEPPING_PORT      PORTC 
diff --git a/main.c b/main.c
index 60349cf..e156a14 100644
--- a/main.c
+++ b/main.c
@@ -20,6 +20,7 @@
 
 #include <avr/io.h>
 #include <avr/sleep.h>
+#include <avr/interrupt.h>
 #include <util/delay.h>
 #include "planner.h"
 #include "stepper.h"
@@ -44,7 +45,9 @@ int main(void)
   st_init();        
   spindle_init();   
   gc_init();
-  limits_init();  
+  limits_init();
+
+  sei();
                     
   for(;;){
     sleep_mode(); // Wait for it ...
diff --git a/stepper.c b/stepper.c
index f011a6a..802f8ac 100644
--- a/stepper.c
+++ b/stepper.c
@@ -43,9 +43,6 @@
 
 #define MINIMUM_STEPS_PER_MINUTE 1200 // The stepper subsystem will never run slower than this, exept when sleeping
 
-#define ENABLE_STEPPER_DRIVER_INTERRUPT()  TIMSK1 |= (1<<OCIE1A)
-#define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A)
-
 static block_t *current_block;  // A pointer to the block currently being traced
 
 // Variables used by The Stepper Driver Interrupt
@@ -82,8 +79,17 @@ static uint32_t trapezoid_adjusted_rate;      // The current rate of step_events
 static void set_step_events_per_minute(uint32_t steps_per_minute);
 
 void st_wake_up() {
-  STEPPERS_ENABLE_PORT |= (1<<STEPPERS_ENABLE_BIT);
-  ENABLE_STEPPER_DRIVER_INTERRUPT();  
+  // Enable steppers by resetting the stepper disable port
+  STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
+  // Enable stepper driver interrupt
+  TIMSK1 |= (1<<OCIE1A);
+}
+
+static void st_go_idle() {
+  // Disable steppers by setting stepper disable
+  STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT);
+  // Disable stepper driver interrupt
+  TIMSK1 &= ~(1<<OCIE1A); 
 }
 
 // Initializes the trapezoid generator from the current block. Called whenever a new 
@@ -154,9 +160,7 @@ SIGNAL(TIMER1_COMPA_vect)
       counter_z = counter_x;
       step_events_completed = 0;
     } else {
-      // set enable pin     
-      STEPPERS_ENABLE_PORT &= ~(1<<STEPPERS_ENABLE_BIT);
-      DISABLE_STEPPER_DRIVER_INTERRUPT();
+      st_go_idle();
     }    
   } 
 
@@ -214,7 +218,7 @@ void st_init()
 	// Configure directions of interface pins
   STEPPING_DDR   |= STEPPING_MASK;
   STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask;
-  STEPPERS_ENABLE_DDR |= 1<<STEPPERS_ENABLE_BIT;
+  STEPPERS_DISABLE_DDR |= 1<<STEPPERS_DISABLE_BIT;
   
 	// waveform generation = 0100 = CTC
 	TCCR1B &= ~(1<<WGM13);
@@ -233,11 +237,9 @@ void st_init()
   
   set_step_events_per_minute(6000);
   trapezoid_tick_cycle_counter = 0;
-
-  STEPPERS_ENABLE_PORT &= ~(1<<STEPPERS_ENABLE_BIT);
-  DISABLE_STEPPER_DRIVER_INTERRUPT();  
-       
-  sei();
+  
+  // Start in the idle state
+  st_go_idle();
 }
 
 // Block until all buffered steps are executed