Fixes for COREXY and COREXZ coordinates

Marlin/Marlin_main.cpp

       st_synchronize();
 
       // Tell the planner where we ended up - Get this from the stepper handler
-      zPosition = st_get_position_mm(Z_AXIS);
+      zPosition = st_get_axis_position_mm(Z_AXIS);
       plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS]);
 
       // move up the retract distance
       endstops_hit_on_purpose(); // clear endstop hit flags
 
       // Get the current stepper position after bumping an endstop
-      current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
+      current_position[Z_AXIS] = st_get_axis_position_mm(Z_AXIS);
       sync_plan_position();
 
       #if ENABLED(DEBUG_LEVELING_FEATURE)
         float x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER,
               y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER,
               z_tmp = current_position[Z_AXIS],
-              real_z = st_get_position_mm(Z_AXIS);  //get the real Z (since plan_get_position is now correcting the plane)
+              real_z = st_get_axis_position_mm(Z_AXIS);  //get the real Z (since plan_get_position is now correcting the plane)
 
         #if ENABLED(DEBUG_LEVELING_FEATURE)
           if (marlin_debug_flags & DEBUG_LEVELING) {
       }
     }
 
-    double X_current = st_get_position_mm(X_AXIS),
-           Y_current = st_get_position_mm(Y_AXIS),
-           Z_current = st_get_position_mm(Z_AXIS),
-           E_current = st_get_position_mm(E_AXIS),
+    double X_current = st_get_axis_position_mm(X_AXIS),
+           Y_current = st_get_axis_position_mm(Y_AXIS),
+           Z_current = st_get_axis_position_mm(Z_AXIS),
+           E_current = st_get_axis_position_mm(E_AXIS),
            X_probe_location = X_current, Y_probe_location = Y_current,
            Z_start_location = Z_current + Z_RAISE_BEFORE_PROBING;
 
                      active_extruder);
     st_synchronize();
 
-    current_position[X_AXIS] = X_current = st_get_position_mm(X_AXIS);
-    current_position[Y_AXIS] = Y_current = st_get_position_mm(Y_AXIS);
-    current_position[Z_AXIS] = Z_current = st_get_position_mm(Z_AXIS);
-    current_position[E_AXIS] = E_current = st_get_position_mm(E_AXIS);
+    current_position[X_AXIS] = X_current = st_get_axis_position_mm(X_AXIS);
+    current_position[Y_AXIS] = Y_current = st_get_axis_position_mm(Y_AXIS);
+    current_position[Z_AXIS] = Z_current = st_get_axis_position_mm(Z_AXIS);
+    current_position[E_AXIS] = E_current = st_get_axis_position_mm(E_AXIS);
 
     //
     // OK, do the initial probe to get us close to the bed.
     setup_for_endstop_move();
     run_z_probe();
 
-    current_position[Z_AXIS] = Z_current = st_get_position_mm(Z_AXIS);
-    Z_start_location = st_get_position_mm(Z_AXIS) + Z_RAISE_BEFORE_PROBING;
+    Z_current = current_position[Z_AXIS] = st_get_axis_position_mm(Z_AXIS);
+    Z_start_location = Z_current + Z_RAISE_BEFORE_PROBING;
 
     plan_buffer_line(X_probe_location, Y_probe_location, Z_start_location,
                      E_current,
                      homing_feedrate[X_AXIS] / 60,
                      active_extruder);
     st_synchronize();
-    current_position[Z_AXIS] = Z_current = st_get_position_mm(Z_AXIS);
+    Z_current = current_position[Z_AXIS] = st_get_axis_position_mm(Z_AXIS);
 
     if (deploy_probe_for_each_reading) stow_z_probe();
 

Marlin/planner.cpp

 
 #if ENABLED(AUTO_BED_LEVELING_FEATURE) && DISABLED(DELTA)
   vector_3 plan_get_position() {
-    vector_3 position = vector_3(st_get_position_mm(X_AXIS), st_get_position_mm(Y_AXIS), st_get_position_mm(Z_AXIS));
+    vector_3 position = vector_3(st_get_axis_position_mm(X_AXIS), st_get_axis_position_mm(Y_AXIS), st_get_axis_position_mm(Z_AXIS));
 
     //position.debug("in plan_get position");
     //plan_bed_level_matrix.debug("in plan_get_position");

Marlin/stepper.cpp

 
 static bool check_endstops = true;
 
-volatile long count_position[NUM_AXIS] = { 0 };
-volatile signed char count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
+volatile long count_position[NUM_AXIS] = { 0 }; // Positions of stepper motors, in step units
+volatile signed char count_direction[NUM_AXIS] = { 1 };
 
 
 //===========================================================================
   }
 }
 
+#if ENABLED(COREXY) || ENABLED(COREXZ)
+  #if ENABLED(COREXY)
+    #define CORE_AXIS_2 B_AXIS
+  #else
+    #define CORE_AXIS_2 C_AXIS
+  #endif
+#endif
+
 void enable_endstops(bool check) { check_endstops = check; }
 
-// Check endstops
+// Check endstops - called from ISR!
 inline void update_endstops() {
 
   #if ENABLED(Z_DUAL_ENDSTOPS)
   // TEST_ENDSTOP: test the old and the current status of an endstop
   #define TEST_ENDSTOP(ENDSTOP) (TEST(current_endstop_bits, ENDSTOP) && TEST(old_endstop_bits, ENDSTOP))
 
-  #define UPDATE_ENDSTOP(AXIS,MINMAX) \
-    SET_ENDSTOP_BIT(AXIS, MINMAX); \
-    if (TEST_ENDSTOP(_ENDSTOP(AXIS, MINMAX))  && (current_block->steps[_AXIS(AXIS)] > 0)) { \
-      endstops_trigsteps[_AXIS(AXIS)] = count_position[_AXIS(AXIS)]; \
-      _ENDSTOP_HIT(AXIS); \
-      step_events_completed = current_block->step_event_count; \
-    }
+  #if ENABLED(COREXY) || ENABLED(COREXZ)
 
-  #if ENABLED(COREXY)
-    // Head direction in -X axis for CoreXY bots.
-    // If DeltaX == -DeltaY, the movement is only in 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))
-  #elif ENABLED(COREXZ)
-    // Head direction in -X axis for CoreXZ bots.
-    // If DeltaX == -DeltaZ, the movement is only in Z axis
-    if ((current_block->steps[A_AXIS] != current_block->steps[C_AXIS]) || (TEST(out_bits, A_AXIS) == TEST(out_bits, C_AXIS))) {
+    #define _SET_TRIGSTEPS(AXIS) do { \
+        float axis_pos = count_position[_AXIS(AXIS)]; \
+        if (_AXIS(AXIS) == A_AXIS) \
+          axis_pos = (axis_pos + count_position[CORE_AXIS_2]) / 2; \
+        else if (_AXIS(AXIS) == CORE_AXIS_2) \
+          axis_pos = (count_position[A_AXIS] - axis_pos) / 2; \
+        endstops_trigsteps[_AXIS(AXIS)] = axis_pos; \
+      } while(0)
+
+  #else
+
+    #define _SET_TRIGSTEPS(AXIS) endstops_trigsteps[_AXIS(AXIS)] = count_position[_AXIS(AXIS)]
+
+  #endif // COREXY || COREXZ
+
+  #define UPDATE_ENDSTOP(AXIS,MINMAX) do { \
+      SET_ENDSTOP_BIT(AXIS, MINMAX); \
+      if (TEST_ENDSTOP(_ENDSTOP(AXIS, MINMAX)) && current_block->steps[_AXIS(AXIS)] > 0) { \
+        _SET_TRIGSTEPS(AXIS); \
+        _ENDSTOP_HIT(AXIS); \
+        step_events_completed = current_block->step_event_count; \
+      } \
+    } while(0)
+
+  #if ENABLED(COREXY) || ENABLED(COREXZ)
+    // Head direction in -X axis for CoreXY and CoreXZ bots.
+    // If Delta1 == -Delta2, the movement is only in Y or Z axis
+    if ((current_block->steps[A_AXIS] != current_block->steps[CORE_AXIS_2]) || (TEST(out_bits, A_AXIS) == TEST(out_bits, CORE_AXIS_2))) {
       if (TEST(out_bits, X_HEAD))
   #else
     if (TEST(out_bits, X_AXIS))   // stepping along -X axis (regular Cartesian bot)
 }
 
 long st_get_position(uint8_t axis) {
-  long count_pos;
   CRITICAL_SECTION_START;
-  count_pos = count_position[axis];
+  long count_pos = count_position[axis];
   CRITICAL_SECTION_END;
   return count_pos;
 }
 
-float st_get_position_mm(AxisEnum axis) { return st_get_position(axis) / axis_steps_per_unit[axis]; }
+float st_get_axis_position_mm(AxisEnum axis) {
+  float axis_pos;
+  #if ENABLED(COREXY) | ENABLED(COREXZ)
+    if (axis == X_AXIS || axis == CORE_AXIS_2) {
+      CRITICAL_SECTION_START;
+      long pos1 = count_position[A_AXIS],
+           pos2 = count_position[CORE_AXIS_2];
+      CRITICAL_SECTION_END;
+      // ((a1+a2)+(a1-a2))/2 -> (a1+a2+a1-a2)/2 -> (a1+a1)/2 -> a1
+      // ((a1+a2)-(a1-a2))/2 -> (a1+a2-a1+a2)/2 -> (a2+a2)/2 -> a2
+      axis_pos = (pos1 + ((axis == X_AXIS) ? pos2 : -pos2)) / 2.0f;
+    }
+    else
+      axis_pos = st_get_position(axis);
+  #else
+    axis_pos = st_get_position(axis);
+  #endif
+  return axis_pos / axis_steps_per_unit[axis];
+}
 
 void finishAndDisableSteppers() {
   st_synchronize();