diff --git a/grbl/config.h b/grbl/config.h
index f48d958..ed4d240 100644
--- a/grbl/config.h
+++ b/grbl/config.h
@@ -336,7 +336,7 @@
 // enable pin will output 5V for maximum RPM with 256 intermediate levels and 0V when disabled.
 // NOTE: IMPORTANT for Arduino Unos! When enabled, the Z-limit pin D11 and spindle enable pin D12 switch!
 // The hardware PWM output on pin D11 is required for variable spindle output voltages.
-#define VARIABLE_SPINDLE // Default enabled. Comment to disable.
+//#define VARIABLE_SPINDLE // Default enabled. Comment to disable.
 
 // Used by variable spindle output only. This forces the PWM output to a minimum duty cycle when enabled.
 // The PWM pin will still read 0V when the spindle is disabled. Most users will not need this option, but
diff --git a/grbl/cpu_map.h b/grbl/cpu_map.h
index 9f43a3f..637c5f6 100644
--- a/grbl/cpu_map.h
+++ b/grbl/cpu_map.h
@@ -36,23 +36,23 @@
   // Define step pulse output pins. NOTE: All step bit pins must be on the same port.
   #define STEP_DDR        DDRD
   #define STEP_PORT       PORTD
-  #define X_STEP_BIT      2  // Uno Digital Pin 2
-  #define Y_STEP_BIT      3  // Uno Digital Pin 3
-  #define Z_STEP_BIT      4  // Uno Digital Pin 4
+  #define X_STEP_BIT      0//2  // Uno Digital Pin 2
+  #define Y_STEP_BIT      0//3  // Uno Digital Pin 3
+  #define Z_STEP_BIT      0//4  // Uno Digital Pin 4
   #define STEP_MASK       ((1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)) // All step bits
 
   // Define step direction output pins. NOTE: All direction pins must be on the same port.
   #define DIRECTION_DDR     DDRD
   #define DIRECTION_PORT    PORTD
-  #define X_DIRECTION_BIT   5  // Uno Digital Pin 5
-  #define Y_DIRECTION_BIT   6  // Uno Digital Pin 6
-  #define Z_DIRECTION_BIT   7  // Uno Digital Pin 7
+  #define X_DIRECTION_BIT   6//5  // Uno Digital Pin 5
+  #define Y_DIRECTION_BIT   7//6  // Uno Digital Pin 6
+  #define Z_DIRECTION_BIT   0//7  // Uno Digital Pin 7
   #define DIRECTION_MASK    ((1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT)) // All direction bits
 
   // Define stepper driver enable/disable output pin.
   #define STEPPERS_DISABLE_DDR    DDRB
   #define STEPPERS_DISABLE_PORT   PORTB
-  #define STEPPERS_DISABLE_BIT    0  // Uno Digital Pin 8
+  #define STEPPERS_DISABLE_BIT    5//0  // Uno Digital Pin 8
   #define STEPPERS_DISABLE_MASK   (1<<STEPPERS_DISABLE_BIT)
 
   // Define homing/hard limit switch input pins and limit interrupt vectors.
@@ -79,10 +79,10 @@
   #define CONTROL_DDR       DDRC
   #define CONTROL_PIN       PINC
   #define CONTROL_PORT      PORTC
-  #define CONTROL_RESET_BIT         0  // Uno Analog Pin 0
-  #define CONTROL_FEED_HOLD_BIT     1  // Uno Analog Pin 1
-  #define CONTROL_CYCLE_START_BIT   2  // Uno Analog Pin 2
-  #define CONTROL_SAFETY_DOOR_BIT   1  // Uno Analog Pin 1 NOTE: Safety door is shared with feed hold. Enabled by config define.
+  #define CONTROL_RESET_BIT         6//0  // Uno Analog Pin 0
+  #define CONTROL_FEED_HOLD_BIT     6//1  // Uno Analog Pin 1
+  #define CONTROL_CYCLE_START_BIT   6//2  // Uno Analog Pin 2
+  #define CONTROL_SAFETY_DOOR_BIT   6//1  // Uno Analog Pin 1 NOTE: Safety door is shared with feed hold. Enabled by config define.
   #define CONTROL_INT       PCIE1  // Pin change interrupt enable pin
   #define CONTROL_INT_vect  PCINT1_vect
   #define CONTROL_PCMSK     PCMSK1 // Pin change interrupt register
diff --git a/grbl/main.c b/grbl/main.c
index 96f2aba..b88e0cc 100644
--- a/grbl/main.c
+++ b/grbl/main.c
@@ -38,6 +38,10 @@ volatile uint8_t sys_rt_exec_accessory_override; // Global realtime executor bit
 
 int main(void)
 {
+   //Inititialize costycnc port
+  DDRD=0B111100;
+  DDRC=0B00001111;
+  
   // Initialize system upon power-up.
   serial_init();   // Setup serial baud rate and interrupts
   settings_init(); // Load Grbl settings from EEPROM
diff --git a/grbl/stepper.c b/grbl/stepper.c
index 8e0b151..d3d5fb6 100644
--- a/grbl/stepper.c
+++ b/grbl/stepper.c
@@ -21,7 +21,6 @@
 
 #include "grbl.h"
 
-
 // Some useful constants.
 #define DT_SEGMENT (1.0/(ACCELERATION_TICKS_PER_SECOND*60.0)) // min/segment
 #define REQ_MM_INCREMENT_SCALAR 1.25
@@ -56,6 +55,17 @@
   #endif
 #endif
 
+uint8_t stepper_pos[8] = {
+  0b0001,
+  0b0011,
+  0b0010,
+  0b0110,
+  0b0100,
+  0b1100,
+  0b1000,
+  0b1001
+};
+
 
 // Stores the planner block Bresenham algorithm execution data for the segments in the segment
 // buffer. Normally, this buffer is partially in-use, but, for the worst case scenario, it will
@@ -425,6 +435,7 @@ ISR(TIMER1_COMPA_vect)
     st.counter_x -= st.exec_block->step_event_count;
     if (st.exec_block->direction_bits & (1<<X_DIRECTION_BIT)) { sys_position[X_AXIS]--; }
     else { sys_position[X_AXIS]++; }
+    PORTD = stepper_pos[sys_position[X_AXIS] & 0b111] << 2;
   }
   #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
     st.counter_y += st.steps[Y_AXIS];
@@ -439,8 +450,10 @@ ISR(TIMER1_COMPA_vect)
     st.counter_y -= st.exec_block->step_event_count;
     if (st.exec_block->direction_bits & (1<<Y_DIRECTION_BIT)) { sys_position[Y_AXIS]--; }
     else { sys_position[Y_AXIS]++; }
+    PORTC = stepper_pos[sys_position[Y_AXIS] & 0b111];
   }
-  #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
+  // Completely disable the Z axis
+  /*#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
     st.counter_z += st.steps[Z_AXIS];
   #else
     st.counter_z += st.exec_block->steps[Z_AXIS];
@@ -450,7 +463,7 @@ ISR(TIMER1_COMPA_vect)
     st.counter_z -= st.exec_block->step_event_count;
     if (st.exec_block->direction_bits & (1<<Z_DIRECTION_BIT)) { sys_position[Z_AXIS]--; }
     else { sys_position[Z_AXIS]++; }
-  }
+  }*/
 
   // During a homing cycle, lock out and prevent desired axes from moving.
   if (sys.state == STATE_HOMING) {