@ -133,9 +133,14 @@ float Planner::max_feedrate_mm_s[XYZE_N], // (mm/s) M203 XYZE - Max speeds
float Planner : : max_e_jerk ; float Planner : : max_e_jerk ;
# endif # endif
# endif # endif
# endif
# if HAS_CLASSIC_JERK
# if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE)
float Planner : : max_jerk [ XYZ ] ; // (mm/s^2) M205 XYZ - The largest speed change requiring no acceleration.
# else # else
float Planner : : max_jerk [ XYZE ] ; // (mm/s^2) M205 XYZE - The largest speed change requiring no acceleration.
float Planner : : max_jerk [ XYZE ] ; // (mm/s^2) M205 XYZE - The largest speed change requiring no acceleration.
# endif # endif
# endif
# if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED) # if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
bool Planner : : abort_on_endstop_hit = false ; bool Planner : : abort_on_endstop_hit = false ;
@ -220,6 +225,10 @@ float Planner::previous_speed[NUM_AXIS],
float Planner : : position_float [ XYZE ] ; // Needed for accurate maths. Steps cannot be used!
float Planner : : position_float [ XYZE ] ; // Needed for accurate maths. Steps cannot be used!
# endif # endif
# if IS_KINEMATIC
float Planner : : position_cart [ XYZE ] ;
# endif
# if ENABLED(ULTRA_LCD) # if ENABLED(ULTRA_LCD)
volatile uint32_t Planner : : block_buffer_runtime_us = 0 ; volatile uint32_t Planner : : block_buffer_runtime_us = 0 ;
# endif # endif
@ -235,6 +244,9 @@ void Planner::init() {
# if HAS_POSITION_FLOAT # if HAS_POSITION_FLOAT
ZERO ( position_float ) ; ZERO ( position_float ) ;
# endif # endif
# if IS_KINEMATIC
ZERO ( position_cart ) ;
# endif
ZERO ( previous_speed ) ; ZERO ( previous_speed ) ;
previous_nominal_speed_sqr = 0 ; previous_nominal_speed_sqr = 0 ;
# if ABL_PLANAR # if ABL_PLANAR
@ -1354,17 +1366,12 @@ void Planner::check_axes_activity() {
} }
# endif # endif
# if PLANNER_LEVELING || HAS_UBL_AND_CURVES # if HAS_LEVELING
/**
/**
* rx , ry , rz - Cartesian positions in mm * rx , ry , rz - Cartesian positions in mm
* Leveled XYZ on completion * Leveled XYZ on completion
*/ */
void Planner : : apply_leveling ( float & rx , float & ry , float & rz ) { void Planner : : apply_leveling ( float & rx , float & ry , float & rz ) {
# if ENABLED(SKEW_CORRECTION)
skew ( rx , ry , rz ) ;
# endif
if ( ! leveling_active ) return ; if ( ! leveling_active ) return ;
# if ABL_PLANAR # if ABL_PLANAR
@ -1406,10 +1413,6 @@ void Planner::check_axes_activity() {
# endif # endif
} }
# endif
# if PLANNER_LEVELING
void Planner : : unapply_leveling ( float raw [ XYZ ] ) { void Planner : : unapply_leveling ( float raw [ XYZ ] ) {
if ( leveling_active ) { if ( leveling_active ) {
@ -1456,7 +1459,23 @@ void Planner::check_axes_activity() {
# endif # endif
} }
# endif // PLANNER_LEVELING
# endif // HAS_LEVELING
# if ENABLED(FWRETRACT)
/**
* rz , e - Cartesian positions in mm
*/
void Planner : : apply_retract ( float & rz , float & e ) {
rz + = fwretract . current_hop ;
e - = fwretract . current_retract [ active_extruder ] ;
}
void Planner : : unapply_retract ( float & rz , float & e ) {
rz - = fwretract . current_hop ;
e + = fwretract . current_retract [ active_extruder ] ;
}
# endif
void Planner : : quick_stop ( ) { void Planner : : quick_stop ( ) {
@ -1554,6 +1573,7 @@ void Planner::synchronize() {
* Add a new linear movement to the planner queue ( in terms of steps ) . * Add a new linear movement to the planner queue ( in terms of steps ) .
* *
* target - target position in steps units * target - target position in steps units
* target_float - target position in direct ( mm , degrees ) units . optional
* fr_mm_s - ( target ) speed of the move * fr_mm_s - ( target ) speed of the move
* extruder - target extruder * extruder - target extruder
* millimeters - the length of the movement , if known * millimeters - the length of the movement , if known
@ -1562,7 +1582,10 @@ void Planner::synchronize() {
*/ */
bool Planner : : _buffer_steps ( const int32_t ( & target ) [ XYZE ] bool Planner : : _buffer_steps ( const int32_t ( & target ) [ XYZE ]
# if HAS_POSITION_FLOAT # if HAS_POSITION_FLOAT
, const float ( & target_float ) [ XYZE ] , const float ( & target_float ) [ ABCE ]
# endif
# if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float ( & delta_mm_cart ) [ XYZE ]
# endif # endif
, float fr_mm_s , const uint8_t extruder , const float & millimeters , float fr_mm_s , const uint8_t extruder , const float & millimeters
) { ) {
@ -1579,6 +1602,9 @@ bool Planner::_buffer_steps(const int32_t (&target)[XYZE]
# if HAS_POSITION_FLOAT # if HAS_POSITION_FLOAT
, target_float , target_float
# endif # endif
# if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, delta_mm_cart
# endif
, fr_mm_s , extruder , millimeters , fr_mm_s , extruder , millimeters
) ) { ) ) {
// Movement was not queued, probably because it was too short.
// Movement was not queued, probably because it was too short.
@ -1618,9 +1644,12 @@ bool Planner::_buffer_steps(const int32_t (&target)[XYZE]
* Returns true is movement is acceptable , false otherwise * Returns true is movement is acceptable , false otherwise
*/ */
bool Planner : : _populate_block ( block_t * const block , bool split_move , bool Planner : : _populate_block ( block_t * const block , bool split_move ,
const int32_t ( & target ) [ XYZ E] const int32_t ( & target ) [ ABC E]
# if HAS_POSITION_FLOAT # if HAS_POSITION_FLOAT
, const float ( & target_float ) [ XYZE ] , const float ( & target_float ) [ ABCE ]
# endif
# if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float ( & delta_mm_cart ) [ XYZE ]
# endif # endif
, float fr_mm_s , const uint8_t extruder , const float & millimeters /*=0.0*/ , float fr_mm_s , const uint8_t extruder , const float & millimeters /*=0.0*/
) { ) {
@ -2243,12 +2272,21 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
// Unit vector of previous path line segment
// Unit vector of previous path line segment
static float previous_unit_vec [ XYZE ] ; static float previous_unit_vec [ XYZE ] ;
# if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
float unit_vec [ ] = { float unit_vec [ ] = {
delta_mm [ A_AXIS ] * inverse_millimeters , delta_mm_cart [ X_AXIS ] * inverse_millimeters ,
delta_mm [ B_AXIS ] * inverse_millimeters , delta_mm_cart [ Y_AXIS ] * inverse_millimeters ,
delta_mm [ C_AXIS ] * inverse_millimeters , delta_mm_cart [ Z_AXIS ] * inverse_millimeters ,
delta_mm_cart [ E_AXIS ] * inverse_millimeters
} ;
# else
float unit_vec [ ] = {
delta_mm [ X_AXIS ] * inverse_millimeters ,
delta_mm [ Y_AXIS ] * inverse_millimeters ,
delta_mm [ Z_AXIS ] * inverse_millimeters ,
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.
if ( moves_queued & & ! UNEAR_ZERO ( previous_nominal_speed_sqr ) ) { if ( moves_queued & & ! UNEAR_ZERO ( previous_nominal_speed_sqr ) ) {
@ -2302,7 +2340,9 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
COPY ( previous_unit_vec , unit_vec ) ; COPY ( previous_unit_vec , unit_vec ) ;
# else // Classic Jerk Limiting
# endif
# if HAS_CLASSIC_JERK
/**
/**
* Adapted from Průša MKS firmware * Adapted from Průša MKS firmware
@ -2317,7 +2357,12 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
float safe_speed = nominal_speed ; float safe_speed = nominal_speed ;
uint8_t limited = 0 ; uint8_t limited = 0 ;
LOOP_XYZE ( i ) { # if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE)
LOOP_XYZ ( i )
# else
LOOP_XYZE ( i )
# endif
{
const float jerk = ABS ( current_speed [ i ] ) , // cs : Starting from zero, change in speed for this axis
const float jerk = ABS ( current_speed [ i ] ) , // cs : Starting from zero, change in speed for this axis
maxj = max_jerk [ i ] ; // mj : The max jerk setting for this axis
maxj = max_jerk [ i ] ; // mj : The max jerk setting for this axis
if ( jerk > maxj ) { // cs > mj : New current speed too fast?
if ( jerk > maxj ) { // cs > mj : New current speed too fast?
@ -2349,7 +2394,12 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
// Now limit the jerk in all axes.
// Now limit the jerk in all axes.
const float smaller_speed_factor = vmax_junction / previous_nominal_speed ; const float smaller_speed_factor = vmax_junction / previous_nominal_speed ;
LOOP_XYZE ( axis ) { # if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE)
LOOP_XYZ ( axis )
# else
LOOP_XYZE ( axis )
# endif
{
// Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop.
// Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop.
float v_exit = previous_speed [ axis ] * smaller_speed_factor , float v_exit = previous_speed [ axis ] * smaller_speed_factor ,
v_entry = current_speed [ axis ] ; v_entry = current_speed [ axis ] ;
@ -2381,7 +2431,12 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
vmax_junction = safe_speed ; vmax_junction = safe_speed ;
previous_safe_speed = safe_speed ; previous_safe_speed = safe_speed ;
# if ENABLED(JUNCTION_DEVIATION)
vmax_junction_sqr = MIN ( vmax_junction_sqr , sq ( vmax_junction ) ) ;
# else
vmax_junction_sqr = sq ( vmax_junction ) ; vmax_junction_sqr = sq ( vmax_junction ) ;
# endif
# endif // Classic Jerk Limiting
# endif // Classic Jerk Limiting
@ -2466,7 +2521,12 @@ void Planner::buffer_sync_block() {
* extruder - target extruder * extruder - target extruder
* millimeters - the length of the movement , if known * millimeters - the length of the movement , if known
*/ */
bool Planner : : buffer_segment ( const float & a , const float & b , const float & c , const float & e , const float & fr_mm_s , const uint8_t extruder , const float & millimeters /*=0.0*/ ) { bool Planner : : buffer_segment ( const float & a , const float & b , const float & c , const float & e
# if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float ( & delta_mm_cart ) [ XYZE ]
# endif
, const float & fr_mm_s , const uint8_t extruder , const float & millimeters /*=0.0*/
) {
// If we are cleaning, do not accept queuing of movements
// If we are cleaning, do not accept queuing of movements
if ( cleaning_buffer_counter ) return false ; if ( cleaning_buffer_counter ) return false ;
@ -2534,6 +2594,9 @@ bool Planner::buffer_segment(const float &a, const float &b, const float &c, con
# if HAS_POSITION_FLOAT # if HAS_POSITION_FLOAT
, target_float , target_float
# endif # endif
# if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, delta_mm_cart
# endif
, fr_mm_s , extruder , millimeters , fr_mm_s , extruder , millimeters
) )
) return false ; ) return false ;
@ -2543,24 +2606,84 @@ bool Planner::buffer_segment(const float &a, const float &b, const float &c, con
} // buffer_segment()
} // buffer_segment()
/**
/**
* Directly set the planner XYZ position ( and stepper positions ) * Add a new linear movement to the buffer .
* The target is cartesian , it ' s translated to delta / scara if
* needed .
*
*
* rx , ry , rz , e - target position in mm or degrees
* fr_mm_s - ( target ) speed of the move ( mm / s )
* extruder - target extruder
* millimeters - the length of the movement , if known
* inv_duration - the reciprocal if the duration of the movement , if known ( kinematic only if feeedrate scaling is enabled )
*/
bool Planner : : buffer_line ( const float & rx , const float & ry , const float & rz , const float & e , const float & fr_mm_s , const uint8_t extruder , const float millimeters
# if ENABLED(SCARA_FEEDRATE_SCALING)
, const float & inv_duration
# endif
) {
float raw [ XYZE ] = { rx , ry , rz , e } ;
# if HAS_POSITION_MODIFIERS
apply_modifiers ( raw ) ;
# endif
# if IS_KINEMATIC
const float delta_mm_cart [ ] = {
rx - position_cart [ X_AXIS ] ,
ry - position_cart [ Y_AXIS ] ,
rz - position_cart [ Z_AXIS ]
# if ENABLED(JUNCTION_DEVIATION)
, e - position_cart [ E_AXIS ]
# endif
} ;
float mm = millimeters ;
if ( mm = = 0.0 )
mm = ( delta_mm_cart [ X_AXIS ] ! = 0.0 | | delta_mm_cart [ Y_AXIS ] ! = 0.0 ) ? SQRT ( sq ( delta_mm_cart [ X_AXIS ] ) + sq ( delta_mm_cart [ Y_AXIS ] ) + sq ( delta_mm_cart [ Z_AXIS ] ) ) : fabs ( delta_mm_cart [ Z_AXIS ] ) ;
inverse_kinematics ( raw ) ;
# if ENABLED(SCARA_FEEDRATE_SCALING)
// For SCARA scale the feed rate from mm/s to degrees/s
// i.e., Complete the angular vector in the given time.
const float duration_recip = inv_duration ? inv_duration : fr_mm_s / mm ,
feedrate = HYPOT ( delta [ A_AXIS ] - position_float [ A_AXIS ] , delta [ B_AXIS ] - position_float [ B_AXIS ] ) * duration_recip ;
# else
const float feedrate = fr_mm_s ;
# endif
if ( buffer_segment ( delta [ A_AXIS ] , delta [ B_AXIS ] , delta [ C_AXIS ] , raw [ E_AXIS ]
# if ENABLED(JUNCTION_DEVIATION)
, delta_mm_cart
# endif
, feedrate , extruder , mm
) ) {
position_cart [ X_AXIS ] = rx ;
position_cart [ Y_AXIS ] = ry ;
position_cart [ Z_AXIS ] = rz ;
position_cart [ E_AXIS ] = e ;
return true ;
}
else
return false ;
# else
return buffer_segment ( raw , fr_mm_s , extruder , millimeters ) ;
# endif
} // buffer_line()
/**
* Directly set the planner ABC position ( and stepper positions )
* converting mm ( or angles for SCARA ) into steps . * converting mm ( or angles for SCARA ) into steps .
* *
* On CORE machines stepper ABC will be translated from the given XYZ . * The provided ABC position is in machine units .
*/ */
void Planner : : _set_position_mm ( const float & a , const float & b , const float & c , const float & e ) { void Planner : : set_machine _position_mm ( const float & a , const float & b , const float & c , const float & e ) {
# if ENABLED(DISTINCT_E_FACTORS) # if ENABLED(DISTINCT_E_FACTORS)
last_extruder = active_extruder ; last_extruder = active_extruder ;
# endif # endif
position [ A_AXIS ] = LROUND ( a * axis_steps_per_mm [ A_AXIS ] ) ; position [ A_AXIS ] = LROUND ( a * axis_steps_per_mm [ A_AXIS ] ) ;
position [ B_AXIS ] = LROUND ( b * axis_steps_per_mm [ B_AXIS ] ) ; position [ B_AXIS ] = LROUND ( b * axis_steps_per_mm [ B_AXIS ] ) ;
position [ C_AXIS ] = LROUND ( axis_steps_per_mm [ C_AXIS ] * ( c + ( position [ C_AXIS ] = LROUND ( c * axis_steps_per_mm [ C_AXIS ] ) ;
# if !IS_KINEMATIC && ENABLED(AUTO_BED_LEVELING_UBL)
leveling_active ? ubl . get_z_correction ( a , b ) :
# endif
0 )
) ) ;
position [ E_AXIS ] = LROUND ( e * axis_steps_per_mm [ _EINDEX ] ) ; position [ E_AXIS ] = LROUND ( e * axis_steps_per_mm [ _EINDEX ] ) ;
# if HAS_POSITION_FLOAT # if HAS_POSITION_FLOAT
position_float [ A_AXIS ] = a ; position_float [ A_AXIS ] = a ;
@ -2577,44 +2700,54 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c
stepper . set_position ( position [ A_AXIS ] , position [ B_AXIS ] , position [ C_AXIS ] , position [ E_AXIS ] ) ; stepper . set_position ( position [ A_AXIS ] , position [ B_AXIS ] , position [ C_AXIS ] , position [ E_AXIS ] ) ;
} }
void Planner : : set_position_mm_kinematic ( const float ( & cart ) [ XYZE ] ) { void Planner : : set_position_mm ( const float & rx , const float & ry , const float & rz , const float & e ) {
# if PLANNER_LEVELING float raw [ XYZE ] = { rx , ry , rz , e } ;
float raw [ XYZ ] = { cart [ X_AXIS ] , cart [ Y_AXIS ] , cart [ Z_AXIS ] } ; # if HAS_POSITION_MODIFIERS
apply_leveling ( raw ) ; apply_modifiers ( raw
# else # if HAS_LEVELING
const float ( & raw ) [ XYZE ] = cart ; , true
# endif
) ;
# endif # endif
# if IS_KINEMATIC # if IS_KINEMATIC
position_cart [ X_AXIS ] = rx ;
position_cart [ Y_AXIS ] = ry ;
position_cart [ Z_AXIS ] = rz ;
position_cart [ E_AXIS ] = e ;
inverse_kinematics ( raw ) ; inverse_kinematics ( raw ) ;
_set_position_mm ( delta [ A_AXIS ] , delta [ B_AXIS ] , delta [ C_AXIS ] , cart [ E_AXIS ] ) ; set_machine _position_mm ( delta [ A_AXIS ] , delta [ B_AXIS ] , delta [ C_AXIS ] , raw [ E_AXIS ] ) ;
# else # else
_set_position_mm ( raw [ X_AXIS ] , raw [ Y_AXIS ] , raw [ Z_AXIS ] , cart [ E_AXIS ] ) ; set_machine _position_mm ( raw [ X_AXIS ] , raw [ Y_AXIS ] , raw [ Z_AXIS ] , raw [ E_AXIS ] ) ;
# endif # endif
} }
/**
/**
* Setters for planner position ( also setting stepper position ) . * Setters for planner position ( also setting stepper position ) .
*/ */
void Planner : : set_position_mm ( const AxisEnum axis , const float & v ) { void Planner : : set_e_ position_mm ( const float & e ) {
# if ENABLED(DISTINCT_E_FACTORS) # if ENABLED(DISTINCT_E_FACTORS)
const uint8_t axis_index = axis + ( axis = = E_AXIS ? active_extruder : 0 ) ; const uint8_t axis_index = E_AXIS + active_extruder ;
last_extruder = active_extruder ; last_extruder = active_extruder ;
# else # else
const uint8_t axis_index = axis ; const uint8_t axis_index = E_AXIS ;
# endif # endif
position [ axis ] = LROUND ( axis_steps_per_mm [ axis_index ] * ( v + ( # if ENABLED(FWRETRACT)
# if ENABLED(AUTO_BED_LEVELING_UBL) float e_new = e - fwretract . current_retract [ active_extruder ] ;
axis = = Z_AXIS & & leveling_active ? ubl . get_z_correction ( current_position [ X_AXIS ] , current_position [ Y_AXIS ] ) : # else
const float e_new = e ;
# endif # endif
0 ) position [ E_AXIS ] = LROUND ( axis_steps_per_mm [ axis_index ] * e_new ) ;
) ) ;
# if HAS_POSITION_FLOAT # if HAS_POSITION_FLOAT
position_float [ axis ] = v ; position_float [ E_AXIS ] = e_new ;
# endif
# if IS_KINEMATIC
position_cart [ E_AXIS ] = e ;
# endif # endif
if ( has_blocks_queued ( ) ) if ( has_blocks_queued ( ) )
buffer_sync_block ( ) ; buffer_sync_block ( ) ;
else else
stepper . set_position ( axis , position [ axis ] ) ; stepper . set_position ( E_AXIS , position [ E_AXIS ] ) ;
} }
// Recalculate the steps/s^2 acceleration rates, based on the mm/s^2
// Recalculate the steps/s^2 acceleration rates, based on the mm/s^2
@ -2638,7 +2771,7 @@ void Planner::reset_acceleration_rates() {
// Recalculate position, steps_to_mm if axis_steps_per_mm changes!
// Recalculate position, steps_to_mm if axis_steps_per_mm changes!
void Planner : : refresh_positioning ( ) { void Planner : : refresh_positioning ( ) {
LOOP_XYZE_N ( i ) steps_to_mm [ i ] = 1.0f / axis_steps_per_mm [ i ] ; LOOP_XYZE_N ( i ) steps_to_mm [ i ] = 1.0f / axis_steps_per_mm [ i ] ;
set_position_mm_kinematic ( current_position ) ; set_position_mm ( current_position ) ;
reset_acceleration_rates ( ) ; reset_acceleration_rates ( ) ;
} }
Thomas Moore
6 years ago
Scott Lahteine