@ -182,7 +182,10 @@ float Planner::previous_speed[NUM_AXIS],
# if ENABLED(LIN_ADVANCE)
float Planner : : extruder_advance_k , // Initialized by settings.load()
Planner : : advance_ed_ratio ; // Initialized by settings.load()
Planner : : advance_ed_ratio , // Initialized by settings.load()
Planner : : position_float [ XYZE ] , // Needed for accurate maths. Steps cannot be used!
Planner : : lin_dist_xy ,
Planner : : lin_dist_e ;
# endif
# if ENABLED(ULTRA_LCD)
@ -198,6 +201,9 @@ Planner::Planner() { init(); }
void Planner : : init ( ) {
block_buffer_head = block_buffer_tail = 0 ;
ZERO ( position ) ;
# if ENABLED(LIN_ADVANCE)
ZERO ( position_float ) ;
# endif
ZERO ( previous_speed ) ;
previous_nominal_speed = 0.0 ;
# if ABL_PLANAR
@ -742,7 +748,9 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
SERIAL_ECHOLNPGM ( " steps) " ) ;
//*/
# if ENABLED(PREVENT_COLD_EXTRUSION) || ENABLED(PREVENT_LENGTHY_EXTRUDE)
// If LIN_ADVANCE is disabled then do E move prevention with integers
// Otherwise it's done in _buffer_segment.
# if DISABLED(LIN_ADVANCE) && (ENABLED(PREVENT_COLD_EXTRUSION) || ENABLED(PREVENT_LENGTHY_EXTRUDE))
if ( de ) {
# if ENABLED(PREVENT_COLD_EXTRUSION)
if ( thermalManager . tooColdToExtrude ( extruder ) ) {
@ -761,7 +769,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
}
# endif // PREVENT_LENGTHY_EXTRUDE
}
# endif // PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE
# endif // !LIN_ADVANCE && ( PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE)
// Compute direction bit-mask for this block
uint8_t dm = 0 ;
@ -1102,14 +1110,10 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
}
# endif
// Calculate and limit speed in mm/sec for each axis , calculate minimum acceleration ratio
// Calculate and limit speed in mm/sec for each axis
float current_speed [ NUM_AXIS ] , speed_factor = 1.0 ; // factor <1 decreases speed
float max_stepper_speed = 0 , min_axis_accel_ratio = 1 ; // ratio < 1 means acceleration ramp needed
LOOP_XYZE ( i ) {
const float cs = FABS ( ( current_speed [ i ] = delta_mm [ i ] * inverse_secs ) ) ;
if ( cs > max_jerk [ i ] )
NOMORE ( min_axis_accel_ratio , max_jerk [ i ] / cs ) ;
NOLESS ( max_stepper_speed , cs ) ;
# if ENABLED(DISTINCT_E_FACTORS)
if ( i = = E_AXIS ) i + = extruder ;
# endif
@ -1154,9 +1158,6 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
}
# endif // XY_FREQUENCY_LIMIT
block - > nominal_speed = max_stepper_speed ; // (mm/sec) Always > 0
block - > nominal_rate = CEIL ( block - > step_event_count * inverse_secs ) ; // (step/sec) Always > 0
// Correct the speed
if ( speed_factor < 1.0 ) {
LOOP_XYZE ( i ) current_speed [ i ] * = speed_factor ;
@ -1164,9 +1165,6 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
block - > nominal_rate * = speed_factor ;
}
float safe_speed = block - > nominal_speed * min_axis_accel_ratio ;
static float previous_safe_speed ;
// Compute and limit the acceleration rate for the trapezoid generator.
const float steps_per_mm = block - > step_event_count * inverse_millimeters ;
uint32_t accel ;
@ -1268,6 +1266,32 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
}
# endif
/**
* Adapted from Pr ů š a MKS firmware
* https : //github.com/prusa3d/Prusa-Firmware
*
* Start with a safe speed ( from which the machine may halt to stop immediately ) .
*/
// Exit speed limited by a jerk to full halt of a previous last segment
static float previous_safe_speed ;
float safe_speed = block - > nominal_speed ;
uint8_t limited = 0 ;
LOOP_XYZE ( i ) {
const float jerk = FABS ( current_speed [ i ] ) , maxj = max_jerk [ i ] ;
if ( jerk > maxj ) {
if ( limited ) {
const float mjerk = maxj * block - > nominal_speed ;
if ( jerk * safe_speed > mjerk ) safe_speed = mjerk / jerk ;
}
else {
+ + limited ;
safe_speed = maxj ;
}
}
}
if ( moves_queued & & ! UNEAR_ZERO ( previous_nominal_speed ) ) {
// Estimate a maximum velocity allowed at a joint of two successive segments.
// If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities,
@ -1279,7 +1303,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
// Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
float v_factor = 1 ;
uint8_t limited = 0 ;
limited = 0 ;
// Now limit the jerk in all axes.
const float smaller_speed_factor = vmax_junction / previous_nominal_speed ;
@ -1355,16 +1379,16 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
* In that case , the retract and move will be executed together .
* This leads to too many advance steps due to a huge e_acceleration .
* The math is good , but we must avoid retract moves with advance !
* > 0 : Extruder is running forward ( e . g . , for " Wipe while retracting " ( Slic3r ) or " Combing " ( Cura ) moves )
* lin_ dist_ e > 0 : Extruder is running forward ( e . g . , for " Wipe while retracting " ( Slic3r ) or " Combing " ( Cura ) moves )
*/
block - > use_advance_lead = esteps & & ( block - > steps [ X_AXIS ] | | block - > steps [ Y_AXIS ] )
& & extruder_advance_k
& & ( uint32_t ) esteps ! = block - > step_event_count
& & > 0 ;
& & lin_ dist_ e > 0 ;
if ( block - > use_advance_lead )
block - > abs_adv_steps_multiplier8 = LROUND (
extruder_advance_k
* ( UNEAR_ZERO ( advance_ed_ratio ) ? de * steps_to_mm [ E_AXIS_N ] / HYPOT ( da * steps_to_mm [ X_AXIS ] , db * steps_to_mm [ Y_AXIS ] ) : advance_ed_ratio ) // Use the fixed ratio, if set
* ( UNEAR_ZERO ( advance_ed_ratio ) ? lin_dist_e / lin_dist_xy : advance_ed_ratio ) // Use the fixed ratio, if set
* ( block - > nominal_speed / ( float ) block - > nominal_rate )
* axis_steps_per_mm [ E_AXIS_N ] * 256.0
) ;
@ -1442,16 +1466,69 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con
SERIAL_ECHOLNPGM ( " ) " ) ;
//*/
// DRYRUN ignores all temperature constraints and assures that the extruder is instantly satisfied
if ( DEBUGGING ( DRYRUN ) )
// DRYRUN prevents E moves from taking place
if ( DEBUGGING ( DRYRUN ) ) {
position [ E_AXIS ] = target [ E_AXIS ] ;
# if ENABLED(LIN_ADVANCE)
position_float [ E_AXIS ] = e ;
# endif
}
# if ENABLED(LIN_ADVANCE)
lin_dist_e = e - position_float [ E_AXIS ] ;
# endif
// If LIN_ADVANCE is enabled then do E move prevention with floats
// Otherwise it's done in _buffer_steps.
# if ENABLED(LIN_ADVANCE) && (ENABLED(PREVENT_COLD_EXTRUSION) || ENABLED(PREVENT_LENGTHY_EXTRUDE))
if ( lin_dist_e ) {
# if ENABLED(PREVENT_COLD_EXTRUSION)
if ( thermalManager . tooColdToExtrude ( extruder ) ) {
position_float [ E_AXIS ] = e ; // Behave as if the move really took place, but ignore E part
position [ E_AXIS ] = target [ E_AXIS ] ;
lin_dist_e = 0 ;
SERIAL_ECHO_START ( ) ;
SERIAL_ECHOLNPGM ( MSG_ERR_COLD_EXTRUDE_STOP ) ;
}
# endif // PREVENT_COLD_EXTRUSION
# if ENABLED(PREVENT_LENGTHY_EXTRUDE)
if ( lin_dist_e * e_factor [ extruder ] > ( EXTRUDE_MAXLENGTH ) ) {
position_float [ E_AXIS ] = e ; // Behave as if the move really took place, but ignore E part
position [ E_AXIS ] = target [ E_AXIS ] ;
lin_dist_e = 0 ;
SERIAL_ECHO_START ( ) ;
SERIAL_ECHOLNPGM ( MSG_ERR_LONG_EXTRUDE_STOP ) ;
}
# endif // PREVENT_LENGTHY_EXTRUDE
}
# endif // LIN_ADVANCE && (PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE)
# if ENABLED(LIN_ADVANCE)
if ( lin_dist_e > 0 )
lin_dist_xy = HYPOT ( a - position_float [ X_AXIS ] , b - position_float [ Y_AXIS ] ) ;
# endif
// Always split the first move into two (if not homing or probing)
if ( ! blocks_queued ( ) ) {
# define _BETWEEN(A) (position[A##_AXIS] + target[A##_AXIS]) >> 1
const int32_t between [ XYZE ] = { _BETWEEN ( X ) , _BETWEEN ( Y ) , _BETWEEN ( Z ) , _BETWEEN ( E ) } ;
DISABLE_STEPPER_DRIVER_INTERRUPT ( ) ;
# if ENABLED(LIN_ADVANCE)
lin_dist_xy * = 0.5 ;
lin_dist_e * = 0.5 ;
# endif
_buffer_steps ( between , fr_mm_s , extruder ) ;
# if ENABLED(LIN_ADVANCE)
position_float [ X_AXIS ] = ( position_float [ X_AXIS ] + a ) * 0.5 ;
position_float [ Y_AXIS ] = ( position_float [ Y_AXIS ] + b ) * 0.5 ;
//position_float[Z_AXIS] = (position_float[Z_AXIS] + c) * 0.5;
position_float [ E_AXIS ] = ( position_float [ E_AXIS ] + e ) * 0.5 ;
# endif
const uint8_t next = block_buffer_head ;
_buffer_steps ( target , fr_mm_s , extruder ) ;
SBI ( block_buffer [ next ] . flag , BLOCK_BIT_CONTINUED ) ;
@ -1462,6 +1539,12 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con
stepper . wake_up ( ) ;
# if ENABLED(LIN_ADVANCE)
position_float [ X_AXIS ] = a ;
position_float [ Y_AXIS ] = b ;
//position_float[Z_AXIS] = c;
position_float [ E_AXIS ] = e ;
# endif
} // buffer_segment()
/**
@ -1482,6 +1565,12 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c
nb = position [ Y_AXIS ] = LROUND ( b * axis_steps_per_mm [ Y_AXIS ] ) ,
nc = position [ Z_AXIS ] = LROUND ( c * axis_steps_per_mm [ Z_AXIS ] ) ,
ne = position [ E_AXIS ] = LROUND ( e * axis_steps_per_mm [ _EINDEX ] ) ;
# if ENABLED(LIN_ADVANCE)
position_float [ X_AXIS ] = a ;
position_float [ Y_AXIS ] = b ;
//position_float[Z_AXIS] = c;
position_float [ E_AXIS ] = e ;
# endif
stepper . set_position ( na , nb , nc , ne ) ;
previous_nominal_speed = 0.0 ; // Resets planner junction speeds. Assumes start from rest.
ZERO ( previous_speed ) ;
@ -1506,8 +1595,16 @@ void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) {
* Sync from the stepper positions . ( e . g . , after an interrupted move )
*/
void Planner : : sync_from_steppers ( ) {
LOOP_XYZE ( i )
LOOP_XYZE ( i ) {
position [ i ] = stepper . position ( ( AxisEnum ) i ) ;
# if ENABLED(LIN_ADVANCE)
position_float [ i ] = position [ i ] * steps_to_mm [ i
# if ENABLED(DISTINCT_E_FACTORS)
+ ( i = = E_AXIS ? active_extruder : 0 )
# endif
] ;
# endif
}
}
/**
@ -1521,6 +1618,9 @@ void Planner::set_position_mm(const AxisEnum axis, const float &v) {
const uint8_t axis_index = axis ;
# endif
position [ axis ] = LROUND ( v * axis_steps_per_mm [ axis_index ] ) ;
# if ENABLED(LIN_ADVANCE)
position_float [ axis ] = v ;
# endif
stepper . set_position ( axis , v ) ;
previous_speed [ axis ] = 0.0 ;
}
Scott Lahteine
7 years ago
committed by