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:
Scott Lahteine 2018-05-31 20:09:53 -05:00
parent 6885ca606c
commit 2deff0d9b9
5 changed files with 42 additions and 8 deletions

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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);
} }
} }

View File

@ -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]))