Merge pull request #1658 from thinkyhead/planner_oh_planner

Cleanup of planner code

Marlin/Marlin.h

@@ -178,7 +178,7 @@ void manage_inactivity(bool ignore_stepper_queue=false);
   #define disable_e3() /* nothing */
 #endif
 
-enum AxisEnum {X_AXIS=0, Y_AXIS=1, Z_AXIS=2, E_AXIS=3, X_HEAD=4, Y_HEAD=5}; 
+enum AxisEnum {X_AXIS=0, Y_AXIS=1, A_AXIS=0, B_AXIS=1, Z_AXIS=2, E_AXIS=3, X_HEAD=4, Y_HEAD=5};
 //X_HEAD and Y_HEAD is used for systems that don't have a 1:1 relationship between X_AXIS and X Head movement, like CoreXY bots.
 
 void FlushSerialRequestResend();
@@ -265,7 +265,7 @@ extern int fanSpeed;
   extern bool filament_sensor;  //indicates that filament sensor readings should control extrusion
   extern float filament_width_meas; //holds the filament diameter as accurately measured
   extern signed char measurement_delay[];  //ring buffer to delay measurement
-  extern int delay_index1, delay_index2;  //index into ring buffer
+  extern int delay_index1, delay_index2;  //ring buffer index. used by planner, temperature, and main code
   extern float delay_dist; //delay distance counter
   extern int meas_delay_cm; //delay distance
 #endif

Marlin/configurator/config/Configuration.h

@@ -406,6 +406,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //const bool FIL_RUNOUT_INVERTING = true;  // Should be uncommented and true or false should assigned
 //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================

Marlin/example_configurations/Felix/Configuration.h

@@ -354,6 +354,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //const bool FIL_RUNOUT_INVERTING = true;  // Should be uncommented and true or false should assigned
 //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================

Marlin/example_configurations/Felix/Configuration_DUAL.h

@@ -354,6 +354,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //const bool FIL_RUNOUT_INVERTING = true;  // Should be uncommented and true or false should assigned
 //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================

Marlin/example_configurations/Hephestos/Configuration.h

@@ -380,6 +380,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //const bool FIL_RUNOUT_INVERTING = true;  // Should be uncommented and true or false should assigned
 //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================

Marlin/example_configurations/K8200/Configuration.h

@@ -385,6 +385,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //const bool FIL_RUNOUT_INVERTING = true;  // Should be uncommented and true or false should assigned
 //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================

Marlin/example_configurations/SCARA/Configuration.h

@@ -409,6 +409,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //const bool FIL_RUNOUT_INVERTING = true;  // Should be uncommented and true or false should assigned
 //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================

Marlin/example_configurations/WITBOX/Configuration.h

@@ -379,6 +379,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //const bool FIL_RUNOUT_INVERTING = true;  // Should be uncommented and true or false should assigned
 //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================

Marlin/example_configurations/delta/generic/Configuration.h

@@ -409,6 +409,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //const bool FIL_RUNOUT_INVERTING = true;  // Should be uncommented and true or false should assigned
 //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================

Marlin/example_configurations/delta/kossel_mini/Configuration.h

@@ -410,6 +410,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 //const bool FIL_RUNOUT_INVERTING = true;  // Should be uncommented and true or false should assigned
 //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================

Marlin/example_configurations/makibox/Configuration.h

@@ -377,6 +377,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //const bool FIL_RUNOUT_INVERTING = true;  // Should be uncommented and true or false should assigned
 //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================

Marlin/example_configurations/tvrrug/Round2/Configuration.h

@@ -379,6 +379,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //const bool FIL_RUNOUT_INVERTING = true;  // Should be uncommented and true or false should assigned
 //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================

Marlin/planner.h

@@ -21,20 +21,16 @@
 // This module is to be considered a sub-module of stepper.c. Please don't include 
 // this file from any other module.
 
-#ifndef planner_h
+#ifndef PLANNER_H
-#define planner_h
+#define PLANNER_H
 
 #include "Marlin.h"
 
-#ifdef ENABLE_AUTO_BED_LEVELING
-#include "vector_3.h"
-#endif // ENABLE_AUTO_BED_LEVELING
-
 // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in 
 // the source g-code and may never actually be reached if acceleration management is active.
 typedef struct {
   // Fields used by the bresenham algorithm for tracing the line
-  long steps_x, steps_y, steps_z, steps_e;  // Step count along each axis
+  long steps[NUM_AXIS];                     // Step count along each axis
   unsigned long step_event_count;           // The number of step events required to complete this block
   long accelerate_until;                    // The index of the step event on which to stop acceleration
   long decelerate_after;                    // The index of the step event on which to start decelerating
@@ -71,43 +67,38 @@ typedef struct {
   volatile char busy;
 } block_t;
 
-#ifdef ENABLE_AUTO_BED_LEVELING
+#define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1))
-// this holds the required transform to compensate for bed level
-extern matrix_3x3 plan_bed_level_matrix;
-#endif // #ifdef ENABLE_AUTO_BED_LEVELING
 
 // Initialize the motion plan subsystem      
 void plan_init();
 
-// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in 
+void check_axes_activity();
-// millimaters. Feed rate specifies the speed of the motion.
+
+// Get the number of buffered moves
+extern volatile unsigned char block_buffer_head;
+extern volatile unsigned char block_buffer_tail;
+FORCE_INLINE uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE); }
 
 #if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
-void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder);
   #if defined(ENABLE_AUTO_BED_LEVELING)
-  #ifndef DELTA
+    #include "vector_3.h"
+    // this holds the required transform to compensate for bed level
+    extern matrix_3x3 plan_bed_level_matrix;
     // Get the position applying the bed level matrix if enabled
     vector_3 plan_get_position();
-  #endif
   #endif  // ENABLE_AUTO_BED_LEVELING
-#else
+  // Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in 
-void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder);
+  // millimeters. Feed rate specifies the speed of the motion.
-#endif  // ENABLE_AUTO_BED_LEVELING || MESH_BED_LEVELING
+  void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder);
-
   // Set position. Used for G92 instructions.
-#if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
   void plan_set_position(float x, float y, float z, const float &e);
 #else
+  void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder);
   void plan_set_position(const float &x, const float &y, const float &z, const float &e);
 #endif // ENABLE_AUTO_BED_LEVELING || MESH_BED_LEVELING
 
 void plan_set_e_position(const float &e);
 
-
-
-void check_axes_activity();
-uint8_t movesplanned(); //return the nr of buffered moves
-
 extern unsigned long minsegmenttime;
 extern float max_feedrate[NUM_AXIS]; // set the max speeds
 extern float axis_steps_per_unit[NUM_AXIS];
@@ -129,38 +120,35 @@ extern unsigned long axis_steps_per_sqr_second[NUM_AXIS];
   extern float autotemp_factor;
 #endif
 
-    
+extern block_t block_buffer[BLOCK_BUFFER_SIZE];            // A ring buffer for motion instructions
-
-
-extern block_t block_buffer[BLOCK_BUFFER_SIZE];            // A ring buffer for motion instfructions
 extern volatile unsigned char block_buffer_head;           // Index of the next block to be pushed
 extern volatile unsigned char block_buffer_tail; 
-// Called when the current block is no longer needed. Discards the block and makes the memory
-// availible for new blocks.    
-FORCE_INLINE void plan_discard_current_block()  
-{
-  if (block_buffer_head != block_buffer_tail) {
-    block_buffer_tail = (block_buffer_tail + 1) & (BLOCK_BUFFER_SIZE - 1);  
-  }
-}
-
-// Gets the current block. Returns NULL if buffer empty
-FORCE_INLINE block_t *plan_get_current_block() 
-{
-  if (block_buffer_head == block_buffer_tail) { 
-    return(NULL); 
-  }
-  block_t *block = &block_buffer[block_buffer_tail];
-  block->busy = true;
-  return(block);
-}
 
 // Returns true if the buffer has a queued block, false otherwise
 FORCE_INLINE bool blocks_queued() { return (block_buffer_head != block_buffer_tail); }
 
+// Called when the current block is no longer needed. Discards
+// the block and makes the memory available for new blocks.
+FORCE_INLINE void plan_discard_current_block() {
+  if (blocks_queued())
+    block_buffer_tail = BLOCK_MOD(block_buffer_tail + 1);
+}
+
+// Gets the current block. Returns NULL if buffer empty
+FORCE_INLINE block_t *plan_get_current_block() {
+  if (blocks_queued()) {
+    block_t *block = &block_buffer[block_buffer_tail];
+    block->busy = true;
+    return block;
+  }
+  else
+    return NULL;
+}
+
 #ifdef PREVENT_DANGEROUS_EXTRUDE
   void set_extrude_min_temp(float temp);
 #endif
 
 void reset_acceleration_rates();
-#endif
+
+#endif //PLANNER_H

Marlin/stepper.cpp

@@ -364,7 +364,7 @@ ISR(TIMER1_COMPA_vect) {
       step_events_completed = 0;
 
       #ifdef Z_LATE_ENABLE
-        if (current_block->steps_z > 0) {
+        if (current_block->steps[Z_AXIS] > 0) {
           enable_z();
           OCR1A = 2000; //1ms wait
           return;
@@ -405,7 +405,7 @@ ISR(TIMER1_COMPA_vect) {
 
     #define UPDATE_ENDSTOP(axis,AXIS,minmax,MINMAX) \
       bool axis ##_## minmax ##_endstop = (READ(AXIS ##_## MINMAX ##_PIN) != AXIS ##_## MINMAX ##_ENDSTOP_INVERTING); \
-      if (axis ##_## minmax ##_endstop && old_## axis ##_## minmax ##_endstop && (current_block->steps_## axis > 0)) { \
+      if (axis ##_## minmax ##_endstop && old_## axis ##_## minmax ##_endstop && (current_block->steps[AXIS ##_AXIS] > 0)) { \
         endstops_trigsteps[AXIS ##_AXIS] = count_position[AXIS ##_AXIS]; \
         endstop_## axis ##_hit = true; \
         step_events_completed = current_block->step_event_count; \
@@ -414,13 +414,13 @@ ISR(TIMER1_COMPA_vect) {
 
     // Check X and Y endstops
     if (check_endstops) {
-      #ifndef COREXY
+      #ifdef COREXY
-        if (TEST(out_bits, X_AXIS))   // stepping along -X axis (regular cartesians bot)
-      #else
         // Head direction in -X axis for CoreXY bots.
         // If DeltaX == -DeltaY, the movement is only in Y axis
-        if (current_block->steps_x != current_block->steps_y || (TEST(out_bits, X_AXIS) == TEST(out_bits, Y_AXIS)))      
+        if (current_block->steps[A_AXIS] != current_block->steps[B_AXIS] || (TEST(out_bits, A_AXIS) == TEST(out_bits, B_AXIS)))
           if (TEST(out_bits, X_HEAD))
+      #else
+          if (TEST(out_bits, X_AXIS))   // stepping along -X axis (regular cartesians bot)
       #endif
           { // -direction
             #ifdef DUAL_X_CARRIAGE
@@ -444,13 +444,13 @@ ISR(TIMER1_COMPA_vect) {
                 #endif
               }
           }
-      #ifndef COREXY
+      #ifdef COREXY
-        if (TEST(out_bits, Y_AXIS))   // -direction
-      #else
         // Head direction in -Y axis for CoreXY bots.
         // If DeltaX == DeltaY, the movement is only in X axis
-        if (current_block->steps_x != current_block->steps_y || (TEST(out_bits, X_AXIS) != TEST(out_bits, Y_AXIS)))
+        if (current_block->steps[A_AXIS] != current_block->steps[B_AXIS] || (TEST(out_bits, A_AXIS) != TEST(out_bits, B_AXIS)))
           if (TEST(out_bits, Y_HEAD))
+      #else
+          if (TEST(out_bits, Y_AXIS))   // -direction
       #endif
           { // -direction
             #if defined(Y_MIN_PIN) && Y_MIN_PIN >= 0
@@ -509,7 +509,7 @@ ISR(TIMER1_COMPA_vect) {
       #endif
 
       #ifdef ADVANCE
-        counter_e += current_block->steps_e;
+        counter_e += current_block->steps[E_AXIS];
         if (counter_e > 0) {
           counter_e -= current_block->step_event_count;
           e_steps[current_block->active_extruder] += TEST(out_bits, E_AXIS) ? -1 : 1;
@@ -523,15 +523,14 @@ ISR(TIMER1_COMPA_vect) {
          * instead of doing each in turn. The extra tests add enough
          * lag to allow it work with without needing NOPs
          */
-        counter_x += current_block->steps_x;
+        #define STEP_ADD(axis, AXIS) \
-        if (counter_x > 0) X_STEP_WRITE(HIGH);
+         counter_## axis += current_block->steps[AXIS ##_AXIS]; \
-        counter_y += current_block->steps_y;
+         if (counter_## axis > 0) { AXIS ##_STEP_WRITE(HIGH); }
-        if (counter_y > 0) Y_STEP_WRITE(HIGH);
+        STEP_ADD(x,X);
-        counter_z += current_block->steps_z;
+        STEP_ADD(y,Y);
-        if (counter_z > 0) Z_STEP_WRITE(HIGH);
+        STEP_ADD(z,Z);
         #ifndef ADVANCE
-          counter_e += current_block->steps_e;
+          STEP_ADD(e,E);
-          if (counter_e > 0) E_STEP_WRITE(HIGH);
         #endif
 
         #define STEP_IF_COUNTER(axis, AXIS) \
@@ -551,7 +550,7 @@ ISR(TIMER1_COMPA_vect) {
       #else // !CONFIG_STEPPERS_TOSHIBA
 
         #define APPLY_MOVEMENT(axis, AXIS) \
-          counter_## axis += current_block->steps_## axis; \
+          counter_## axis += current_block->steps[AXIS ##_AXIS]; \
           if (counter_## axis > 0) { \
             AXIS ##_APPLY_STEP(!INVERT_## AXIS ##_STEP_PIN,0); \
             counter_## axis -= current_block->step_event_count; \