|
|
|
@ -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); |
|
|
|
} |
|
|
|
} |
|
|
|
|
Scott Lahteine
7 years ago
GitHub