|
|
@ -2230,21 +2230,13 @@ bool Planner::_populate_block(block_t * const block, bool split_move, |
|
|
already calculated in a different place. */ |
|
|
already calculated in a different place. */ |
|
|
|
|
|
|
|
|
// Unit vector of previous path line segment
|
|
|
// Unit vector of previous path line segment
|
|
|
static float previous_unit_vec[ |
|
|
static float previous_unit_vec[XYZE]; |
|
|
#if ENABLED(JUNCTION_DEVIATION_INCLUDE_E) |
|
|
|
|
|
XYZE |
|
|
|
|
|
#else |
|
|
|
|
|
XYZ |
|
|
|
|
|
#endif |
|
|
|
|
|
]; |
|
|
|
|
|
|
|
|
|
|
|
float unit_vec[] = { |
|
|
float unit_vec[] = { |
|
|
delta_mm[A_AXIS] * inverse_millimeters, |
|
|
delta_mm[A_AXIS] * inverse_millimeters, |
|
|
delta_mm[B_AXIS] * inverse_millimeters, |
|
|
delta_mm[B_AXIS] * inverse_millimeters, |
|
|
delta_mm[C_AXIS] * inverse_millimeters |
|
|
delta_mm[C_AXIS] * inverse_millimeters, |
|
|
#if ENABLED(JUNCTION_DEVIATION_INCLUDE_E) |
|
|
delta_mm[E_AXIS] * inverse_millimeters |
|
|
, delta_mm[E_AXIS] * inverse_millimeters |
|
|
|
|
|
#endif |
|
|
|
|
|
}; |
|
|
}; |
|
|
|
|
|
|
|
|
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
|
|
|
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
|
|
|
@ -2254,9 +2246,7 @@ bool Planner::_populate_block(block_t * const block, bool split_move, |
|
|
float junction_cos_theta = -previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] |
|
|
float junction_cos_theta = -previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] |
|
|
-previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] |
|
|
-previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] |
|
|
-previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] |
|
|
-previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] |
|
|
#if ENABLED(JUNCTION_DEVIATION_INCLUDE_E) |
|
|
|
|
|
-previous_unit_vec[E_AXIS] * unit_vec[E_AXIS] |
|
|
-previous_unit_vec[E_AXIS] * unit_vec[E_AXIS] |
|
|
#endif |
|
|
|
|
|
; |
|
|
; |
|
|
|
|
|
|
|
|
// NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
|
|
|
// NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
|
|
|
@ -2267,15 +2257,13 @@ 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.
|
|
|
|
|
|
|
|
|
float junction_unit_vec[JD_AXES] = { |
|
|
// Convert delta vector to unit vector
|
|
|
|
|
|
float junction_unit_vec[XYZE] = { |
|
|
unit_vec[X_AXIS] - previous_unit_vec[X_AXIS], |
|
|
unit_vec[X_AXIS] - previous_unit_vec[X_AXIS], |
|
|
unit_vec[Y_AXIS] - previous_unit_vec[Y_AXIS], |
|
|
unit_vec[Y_AXIS] - previous_unit_vec[Y_AXIS], |
|
|
unit_vec[Z_AXIS] - previous_unit_vec[Z_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] |
|
|
, unit_vec[E_AXIS] - previous_unit_vec[E_AXIS] |
|
|
|
|
|
#endif |
|
|
|
|
|
}; |
|
|
}; |
|
|
// Convert delta vector to unit vector
|
|
|
|
|
|
normalize_junction_vector(junction_unit_vec); |
|
|
normalize_junction_vector(junction_unit_vec); |
|
|
|
|
|
|
|
|
const float junction_acceleration = limit_value_by_axis_maximum(block->acceleration, junction_unit_vec), |
|
|
const float junction_acceleration = limit_value_by_axis_maximum(block->acceleration, junction_unit_vec), |
|
|
|
Scott Lahteine
7 years ago
GitHub