diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index d21bd599e9..830ffff51e 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1321,7 +1321,7 @@ static void setup_for_endstop_move() {
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
@@ -1339,7 +1339,7 @@ static void setup_for_endstop_move() {
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)
@@ -3143,7 +3143,7 @@ inline void gcode_G28() {
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) {
@@ -3585,10 +3585,10 @@ inline void gcode_M42() {
}
}
- 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;
@@ -3642,10 +3642,10 @@ inline void gcode_M42() {
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.
@@ -3657,15 +3657,15 @@ inline void gcode_M42() {
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();
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index 7f54c8950c..1407c075b2 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -1005,7 +1005,7 @@ float junction_deviation = 0.1;
#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");
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index 395b517863..c275907c68 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -93,8 +93,8 @@ static volatile char endstop_hit_bits = 0; // use X_MIN, Y_MIN, Z_MIN and Z_MIN_
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 };
//===========================================================================
@@ -286,9 +286,17 @@ void checkHitEndstops() {
}
}
+#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)
@@ -311,23 +319,36 @@ inline void update_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)
@@ -1087,14 +1108,31 @@ void st_set_e_position(const long& e) {
}
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();