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.
This commit is contained in:
parent
6885ca606c
commit
2deff0d9b9
5 changed files with 42 additions and 8 deletions
|
@ -447,7 +447,6 @@
|
||||||
//#define JUNCTION_DEVIATION
|
//#define JUNCTION_DEVIATION
|
||||||
#if ENABLED(JUNCTION_DEVIATION)
|
#if ENABLED(JUNCTION_DEVIATION)
|
||||||
#define JUNCTION_DEVIATION_MM 0.02 // (mm) Distance from real junction edge
|
#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
|
//#define JUNCTION_DEVIATION_INCLUDE_E
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -447,7 +447,6 @@
|
||||||
//#define JUNCTION_DEVIATION
|
//#define JUNCTION_DEVIATION
|
||||||
#if ENABLED(JUNCTION_DEVIATION)
|
#if ENABLED(JUNCTION_DEVIATION)
|
||||||
#define JUNCTION_DEVIATION_MM 0.02 // (mm) Distance from real junction edge
|
#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
|
//#define JUNCTION_DEVIATION_INCLUDE_E
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -278,7 +278,9 @@
|
||||||
#elif defined(JUNCTION_DEVIATION_FACTOR)
|
#elif defined(JUNCTION_DEVIATION_FACTOR)
|
||||||
#error "JUNCTION_DEVIATION_FACTOR is now JUNCTION_DEVIATION_MM. Please update your configuration."
|
#error "JUNCTION_DEVIATION_FACTOR is now JUNCTION_DEVIATION_MM. Please update your configuration."
|
||||||
#elif defined(JUNCTION_ACCELERATION_FACTOR)
|
#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
|
#endif
|
||||||
|
|
||||||
#define BOARD_MKS_13 -47
|
#define BOARD_MKS_13 -47
|
||||||
|
|
|
@ -2166,11 +2166,22 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
NOLESS(junction_cos_theta, -0.999999); // Check for numerical round-off to avoid divide by zero.
|
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
|
float junction_unit_vec[JD_AXES] = {
|
||||||
// two junctions. However, this shouldn't be a significant problem except in extreme circumstances.
|
unit_vec[X_AXIS] - previous_unit_vec[X_AXIS],
|
||||||
vmax_junction_sqr = (JUNCTION_ACCELERATION * JUNCTION_DEVIATION_MM * sin_theta_d2) / (1.0 - sin_theta_d2);
|
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) {
|
if (block->millimeters < 1.0) {
|
||||||
|
|
||||||
// Fast acos approximation, minus the error bar to be safe
|
// 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 angle is greater than 135 degrees (octagon), find speed for approximate arc
|
||||||
if (junction_theta > RADIANS(135)) {
|
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);
|
NOMORE(vmax_junction_sqr, limit_sqr);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -802,6 +802,29 @@ class Planner {
|
||||||
|
|
||||||
static void recalculate();
|
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]))
|
#define PLANNER_XY_FEEDRATE() (MIN(planner.max_feedrate_mm_s[X_AXIS], planner.max_feedrate_mm_s[Y_AXIS]))
|
||||||
|
|
Reference in a new issue