From 373e8a1ad4ec9753e54343a766e8fb7adc8c7cbe Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Thu, 11 Feb 2016 18:17:17 -0800 Subject: [PATCH] Fixes for COREXY and COREXZ coordinates --- Marlin/Marlin_main.cpp | 28 +++++++-------- Marlin/planner.cpp | 2 +- Marlin/stepper.cpp | 82 ++++++++++++++++++++++++++++++------------ 3 files changed, 75 insertions(+), 37 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index d21bd599e..830ffff51 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 7f54c8950..1407c075b 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 395b51786..c275907c6 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();