From 568b19e7d4ef337daf69fd7c1aa0a4e4d2fd400b Mon Sep 17 00:00:00 2001
From: Sebastianv650 <sebastian_v650@vodafonemail.de>
Date: Thu, 31 May 2018 08:35:54 +0200
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/SanityCheck.h | 4 +++-
Marlin/planner.cpp | 21 ++++++++++++++++-----
Marlin/planner.h | 24 ++++++++++++++++++++++++
4 files changed, 43 insertions(+), 7 deletions(-)
diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index 9635d3b708a..49840449cb5 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/SanityCheck.h b/Marlin/SanityCheck.h
index 5cc5c4cf394..20506284562 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/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/planner.cpp b/Marlin/planner.cpp
index 94d8bbe387e..4ae9d16d467 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -2148,11 +2148,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
@@ -2160,7 +2171,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/planner.h b/Marlin/planner.h
index 894e83a113c..0fea595d3c9 100644
--- a/Marlin/planner.h
+++ b/Marlin/planner.h
@@ -788,6 +788,30 @@ class Planner {
static void recalculate_trapezoids();
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]))