From 2deff0d9b96e117fc03870169b52ac785d7bf430 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Thu, 31 May 2018 20:09:53 -0500 Subject: [PATCH] Fix/Improve junction deviation - Respect axis max acceleration limits instead of forcing a fixed acceleration value. - The `junction_unit_vec` ensures proper handling of entry and exit speeds even when the axes involved have different limits. --- Marlin/Configuration_adv.h | 1 - Marlin/src/config/default/Configuration_adv.h | 1 - Marlin/src/inc/SanityCheck.h | 4 +++- Marlin/src/module/planner.cpp | 21 +++++++++++++---- Marlin/src/module/planner.h | 23 +++++++++++++++++++ 5 files changed, 42 insertions(+), 8 deletions(-) diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 2a0bc6183..b7a38d487 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -447,7 +447,6 @@ //#define JUNCTION_DEVIATION #if ENABLED(JUNCTION_DEVIATION) #define JUNCTION_DEVIATION_MM 0.02 // (mm) Distance from real junction edge - #define JUNCTION_ACCELERATION 1000 // (mm/s²) Maximum centripetal acceleration //#define JUNCTION_DEVIATION_INCLUDE_E #endif diff --git a/Marlin/src/config/default/Configuration_adv.h b/Marlin/src/config/default/Configuration_adv.h index 2a0bc6183..b7a38d487 100644 --- a/Marlin/src/config/default/Configuration_adv.h +++ b/Marlin/src/config/default/Configuration_adv.h @@ -447,7 +447,6 @@ //#define JUNCTION_DEVIATION #if ENABLED(JUNCTION_DEVIATION) #define JUNCTION_DEVIATION_MM 0.02 // (mm) Distance from real junction edge - #define JUNCTION_ACCELERATION 1000 // (mm/s²) Maximum centripetal acceleration //#define JUNCTION_DEVIATION_INCLUDE_E #endif diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index d37f16b22..8d254c2ec 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -278,7 +278,9 @@ #elif defined(JUNCTION_DEVIATION_FACTOR) #error "JUNCTION_DEVIATION_FACTOR is now JUNCTION_DEVIATION_MM. Please update your configuration." #elif defined(JUNCTION_ACCELERATION_FACTOR) - #error "JUNCTION_ACCELERATION_FACTOR is now JUNCTION_ACCELERATION. Please update your configuration." + #error "JUNCTION_ACCELERATION_FACTOR is obsolete. Delete it from Configuration_adv.h." +#elif defined(JUNCTION_ACCELERATION) + #error "JUNCTION_ACCELERATION is obsolete. Delete it from Configuration_adv.h." #endif #define BOARD_MKS_13 -47 diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 4676d3243..1d358495a 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -2166,11 +2166,22 @@ bool Planner::_populate_block(block_t * const block, bool split_move, } else { NOLESS(junction_cos_theta, -0.999999); // Check for numerical round-off to avoid divide by zero. - const float sin_theta_d2 = SQRT(0.5 * (1.0 - junction_cos_theta)); // Trig half angle identity. Always positive. - // TODO: Technically, the acceleration used in calculation needs to be limited by the minimum of the - // two junctions. However, this shouldn't be a significant problem except in extreme circumstances. - vmax_junction_sqr = (JUNCTION_ACCELERATION * JUNCTION_DEVIATION_MM * sin_theta_d2) / (1.0 - sin_theta_d2); + float junction_unit_vec[JD_AXES] = { + unit_vec[X_AXIS] - previous_unit_vec[X_AXIS], + unit_vec[Y_AXIS] - previous_unit_vec[Y_AXIS], + unit_vec[Z_AXIS] - previous_unit_vec[Z_AXIS] + #if ENABLED(JUNCTION_DEVIATION_INCLUDE_E) + , unit_vec[E_AXIS] - previous_unit_vec[E_AXIS] + #endif + }; + // Convert delta vector to unit vector + normalize_junction_vector(junction_unit_vec); + + const float junction_acceleration = limit_value_by_axis_maximum(block->acceleration, junction_unit_vec), + sin_theta_d2 = SQRT(0.5 * (1.0 - junction_cos_theta)); // Trig half angle identity. Always positive. + + vmax_junction_sqr = (junction_acceleration * JUNCTION_DEVIATION_MM * sin_theta_d2) / (1.0 - sin_theta_d2); if (block->millimeters < 1.0) { // Fast acos approximation, minus the error bar to be safe @@ -2178,7 +2189,7 @@ bool Planner::_populate_block(block_t * const block, bool split_move, // If angle is greater than 135 degrees (octagon), find speed for approximate arc if (junction_theta > RADIANS(135)) { - const float limit_sqr = block->millimeters / (RADIANS(180) - junction_theta) * JUNCTION_ACCELERATION; + const float limit_sqr = block->millimeters / (RADIANS(180) - junction_theta) * junction_acceleration; NOMORE(vmax_junction_sqr, limit_sqr); } } diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index b8ab952c1..8e5d52b09 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -802,6 +802,29 @@ class Planner { static void recalculate(); + #if ENABLED(JUNCTION_DEVIATION) + + #if ENABLED(JUNCTION_DEVIATION_INCLUDE_E) + #define JD_AXES XYZE + #else + #define JD_AXES XYZ + #endif + + FORCE_INLINE static void normalize_junction_vector(float (&vector)[JD_AXES]) { + float magnitude_sq = 0.0; + for (uint8_t idx = 0; idx < JD_AXES; idx++) if (vector[idx]) magnitude_sq += sq(vector[idx]); + const float inv_magnitude = 1.0 / SQRT(magnitude_sq); + for (uint8_t idx = 0; idx < JD_AXES; idx++) vector[idx] *= inv_magnitude; + } + + FORCE_INLINE static float limit_value_by_axis_maximum(const float &max_value, float (&unit_vec)[JD_AXES]) { + float limit_value = max_value; + for (uint8_t idx = 0; idx < JD_AXES; idx++) if (unit_vec[idx]) // Avoid divide by zero + NOMORE(limit_value, ABS(max_acceleration_mm_per_s2[idx] / unit_vec[idx])); + return limit_value; + } + + #endif // JUNCTION_DEVIATION }; #define PLANNER_XY_FEEDRATE() (MIN(planner.max_feedrate_mm_s[X_AXIS], planner.max_feedrate_mm_s[Y_AXIS]))