From 7f265db3ef77df6b87803b5f849ef1827f76b1f0 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sun, 10 Apr 2016 23:40:11 -0700 Subject: [PATCH 1/4] Move sync_plan_position after DEBUG_LEVELING --- Marlin/Marlin_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 0b180c78e2..23f1c2ba36 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -3069,12 +3069,13 @@ inline void gcode_G28() { current_position[X_AXIS] = uncorrected_position.x; current_position[Y_AXIS] = uncorrected_position.y; current_position[Z_AXIS] = uncorrected_position.z; - sync_plan_position(); #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("AFTER matrix.set_to_identity", current_position); #endif + sync_plan_position(); + #endif // !DELTA } From fdee2be49c5258145e269bf2588afa5cc9aae738 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Mon, 11 Apr 2016 00:06:33 -0700 Subject: [PATCH 2/4] More logging of matrix behavior --- Marlin/Marlin_main.cpp | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 23f1c2ba36..69fcc07048 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -1351,8 +1351,14 @@ static void setup_for_endstop_move() { #if DISABLED(DELTA) static void set_bed_level_equation_lsq(double* plane_equation_coefficients) { + #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(LEVELING)) DEBUG_POS("BEFORE set_bed_level_equation_lsq", current_position); + plan_bed_level_matrix.set_to_identity(); + if (DEBUGGING(LEVELING)) { + vector_3 uncorrected_position = plan_get_position(); + DEBUG_POS(">>> set_bed_level_equation_lsq", uncorrected_position); + DEBUG_POS(">>> set_bed_level_equation_lsq", current_position); + } #endif vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1); @@ -1371,7 +1377,7 @@ static void setup_for_endstop_move() { current_position[Z_AXIS] = corrected_position.z; #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(LEVELING)) DEBUG_POS("AFTER set_bed_level_equation_lsq", current_position); + if (DEBUGGING(LEVELING)) DEBUG_POS("<<< set_bed_level_equation_lsq", current_position); #endif sync_plan_position(); @@ -3059,7 +3065,11 @@ inline void gcode_G28() { #else //!DELTA #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(LEVELING)) DEBUG_POS("BEFORE matrix.set_to_identity", current_position); + if (DEBUGGING(LEVELING)) { + vector_3 corrected_position = plan_get_position(); + DEBUG_POS("BEFORE matrix.set_to_identity", corrected_position); + DEBUG_POS("BEFORE matrix.set_to_identity", current_position); + } #endif //vector_3 corrected_position = plan_get_position(); @@ -3071,7 +3081,7 @@ inline void gcode_G28() { current_position[Z_AXIS] = uncorrected_position.z; #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(LEVELING)) DEBUG_POS("AFTER matrix.set_to_identity", current_position); + if (DEBUGGING(LEVELING)) DEBUG_POS("AFTER matrix.set_to_identity", uncorrected_position); #endif sync_plan_position(); From e087a99a10b2c11a34876ea2ba4042a77f918544 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Mon, 11 Apr 2016 01:03:10 -0700 Subject: [PATCH 3/4] Some cleanup of st_get_pos functions --- Marlin/planner.cpp | 12 ++++++++++++ Marlin/stepper.cpp | 19 +++++++++++++------ Marlin/stepper.h | 2 +- 3 files changed, 26 insertions(+), 7 deletions(-) diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index 318b5bdb28..e32c61230e 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -1090,6 +1090,12 @@ float junction_deviation = 0.1; } // plan_buffer_line() #if ENABLED(AUTO_BED_LEVELING_FEATURE) && DISABLED(DELTA) + + /** + * Get the XYZ position of the steppers as a vector_3. + * + * On CORE machines XYZ is derived from ABC. + */ vector_3 plan_get_position() { 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)); @@ -1102,8 +1108,14 @@ float junction_deviation = 0.1; return position; } + #endif // AUTO_BED_LEVELING_FEATURE && !DELTA +/** + * Directly set the planner XYZ position (hence the stepper positions). + * + * On CORE machines stepper ABC will be translated from the given XYZ. + */ #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING) void plan_set_position(float x, float y, float z, const float& e) #else diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp index a371361c28..4bfe74ad15 100644 --- a/Marlin/stepper.cpp +++ b/Marlin/stepper.cpp @@ -1099,15 +1099,22 @@ void st_set_e_position(const long& e) { CRITICAL_SECTION_END; } -long st_get_position(uint8_t axis) { +/** + * Get a stepper's position in steps. + */ +long st_get_position(AxisEnum axis) { CRITICAL_SECTION_START; long count_pos = count_position[axis]; CRITICAL_SECTION_END; return count_pos; } +/** + * Get an axis position according to stepper position(s) + * For CORE machines apply translation from ABC to XYZ. + */ float st_get_axis_position_mm(AxisEnum axis) { - float axis_pos; + float axis_steps; #if ENABLED(COREXY) | ENABLED(COREXZ) if (axis == X_AXIS || axis == CORE_AXIS_2) { CRITICAL_SECTION_START; @@ -1116,14 +1123,14 @@ float st_get_axis_position_mm(AxisEnum axis) { 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; + axis_steps = (pos1 + ((axis == X_AXIS) ? pos2 : -pos2)) / 2.0f; } else - axis_pos = st_get_position(axis); + axis_steps = st_get_position(axis); #else - axis_pos = st_get_position(axis); + axis_steps = st_get_position(axis); #endif - return axis_pos / axis_steps_per_unit[axis]; + return axis_steps / axis_steps_per_unit[axis]; } void finishAndDisableSteppers() { diff --git a/Marlin/stepper.h b/Marlin/stepper.h index aecbf58029..b0230b8cd2 100644 --- a/Marlin/stepper.h +++ b/Marlin/stepper.h @@ -61,7 +61,7 @@ void st_set_position(const long& x, const long& y, const long& z, const long& e) void st_set_e_position(const long& e); // Get current position in steps -long st_get_position(uint8_t axis); +long st_get_position(AxisEnum axis); // Get current axis position in mm float st_get_axis_position_mm(AxisEnum axis); From 3e5312f116bbf08b93d5b26ab2f664f4b2e62dcc Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Mon, 11 Apr 2016 01:03:50 -0700 Subject: [PATCH 4/4] CORE support for st_set_position & plan_set_position --- Marlin/stepper.cpp | 31 ++++++++++++++++++++++++++++--- 1 file changed, 28 insertions(+), 3 deletions(-) diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp index 4bfe74ad15..f8f9312471 100644 --- a/Marlin/stepper.cpp +++ b/Marlin/stepper.cpp @@ -1084,11 +1084,36 @@ void st_init() { */ void st_synchronize() { while (blocks_queued()) idle(); } +/** + * Set the stepper positions directly in steps + * + * The input is based on the typical per-axis XYZ steps. + * For CORE machines XYZ needs to be translated to ABC. + * + * This allows st_get_axis_position_mm to correctly + * derive the current XYZ position later on. + */ void st_set_position(const long& x, const long& y, const long& z, const long& e) { CRITICAL_SECTION_START; - count_position[X_AXIS] = x; - count_position[Y_AXIS] = y; - count_position[Z_AXIS] = z; + + #if ENABLED(COREXY) + // corexy positioning + // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html + count_position[A_AXIS] = x + y; + count_position[B_AXIS] = x - y; + count_position[Z_AXIS] = z; + #elif ENABLED(COREXZ) + // corexz planning + count_position[A_AXIS] = x + z; + count_position[Y_AXIS] = y; + count_position[C_AXIS] = x - z; + #else + // default non-h-bot planning + count_position[X_AXIS] = x; + count_position[Y_AXIS] = y; + count_position[Z_AXIS] = z; + #endif + count_position[E_AXIS] = e; CRITICAL_SECTION_END; }