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