Planner Class Reference

#include <planner.h>

Collaboration diagram for Planner:

Public Member Functions
	Planner ()
void	init ()

Static Public Member Functions
static void	reset_acceleration_rates ()
static void	refresh_positioning ()
static void	set_max_acceleration (const uint8_t axis, float targetValue)
static void	set_max_feedrate (const uint8_t axis, float targetValue)
static void	set_max_jerk (const AxisEnum axis, float targetValue)
static void	check_axes_activity ()
static void	calculate_volumetric_multipliers ()
static FORCE_INLINE float	fade_scaling_factor_for_z (const float &)
static FORCE_INLINE bool	leveling_active_at_z (const float &)
static void	apply_leveling (xyz_pos_t &raw)
static void	unapply_leveling (xyz_pos_t &raw)
static FORCE_INLINE void	force_unapply_leveling (xyz_pos_t &raw)
static FORCE_INLINE void	apply_modifiers (xyze_pos_t &pos, bool leveling=false)
static FORCE_INLINE void	unapply_modifiers (xyze_pos_t &pos, bool leveling=false)
static FORCE_INLINE uint8_t	movesplanned ()
static FORCE_INLINE uint8_t	nonbusy_movesplanned ()
static FORCE_INLINE void	clear_block_buffer ()
static FORCE_INLINE bool	is_full ()
static FORCE_INLINE uint8_t	moves_free ()
static FORCE_INLINE block_t *	get_next_free_block (uint8_t &next_buffer_head, const uint8_t count=1)
static bool	_buffer_steps (const xyze_long_t &target, const xyze_pos_t &target_float, feedRate_t fr_mm_s, const uint8_t extruder, const float &millimeters=0.0)
static bool	_populate_block (block_t *const block, bool split_move, const xyze_long_t &target, const xyze_pos_t &target_float, feedRate_t fr_mm_s, const uint8_t extruder, const float &millimeters=0.0)
static void	buffer_sync_block ()
static bool	buffer_segment (const float &a, const float &b, const float &c, const float &e, const feedRate_t &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0)
static FORCE_INLINE bool	buffer_segment (abce_pos_t &abce, const feedRate_t &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0)
static bool	buffer_line (const float &rx, const float &ry, const float &rz, const float &e, const feedRate_t &fr_mm_s, const uint8_t extruder, const float millimeters=0.0)
static FORCE_INLINE bool	buffer_line (const xyze_pos_t &cart, const feedRate_t &fr_mm_s, const uint8_t extruder, const float millimeters=0.0)
static void	set_position_mm (const float &rx, const float &ry, const float &rz, const float &e)
static FORCE_INLINE void	set_position_mm (const xyze_pos_t &cart)
static void	set_e_position_mm (const float &e)
static void	set_machine_position_mm (const float &a, const float &b, const float &c, const float &e)
static FORCE_INLINE void	set_machine_position_mm (const abce_pos_t &abce)
static float	get_axis_position_mm (const AxisEnum axis)
static void	quick_stop ()
static void	endstop_triggered (const AxisEnum axis)
static float	triggered_position_mm (const AxisEnum axis)
static void	synchronize ()
static void	finish_and_disable ()
static void	tick ()
static FORCE_INLINE bool	has_blocks_queued ()
static block_t *	get_current_block ()
static FORCE_INLINE void	discard_current_block ()

Static Public Attributes
static block_t	block_buffer [BLOCK_BUFFER_SIZE]
static volatile uint8_t	block_buffer_head
static volatile uint8_t	block_buffer_nonbusy
static volatile uint8_t	block_buffer_planned
static volatile uint8_t	block_buffer_tail
static uint16_t	cleaning_buffer_counter
static uint8_t	delay_before_delivering
static planner_settings_t	settings
static uint32_t	max_acceleration_steps_per_s2 [XYZE_N]
static float	steps_to_mm [XYZE_N]
static bool	leveling_active = false
static matrix_3x3	bed_level_matrix
static xyze_pos_t	position_float
static skew_factor_t	skew_factor

Constructor & Destructor Documentation

Planner()

Planner::Planner

(

)

Instance Methods

Class and Instance Methods

{ init(); }

Here is the call graph for this function:

Member Function Documentation

init()

void Planner::init

(

)

                   {
  position.reset();
  #if HAS_POSITION_FLOAT
    position_float.reset();
  #endif
  #if IS_KINEMATIC
    position_cart.reset();
  #endif
  previous_speed.reset();
  previous_nominal_speed_sqr = 0;
  #if ABL_PLANAR
    bed_level_matrix.set_to_identity();
  #endif
  clear_block_buffer();
  delay_before_delivering = 0;
}

Here is the call graph for this function:

Here is the caller graph for this function:

reset_acceleration_rates()

void Planner::reset_acceleration_rates ( )

static

Static (class) Methods

                                       {
  #if ENABLED(DISTINCT_E_FACTORS)
    #define AXIS_CONDITION (i < E_AXIS || i == E_AXIS_N(active_extruder))
  #else
    #define AXIS_CONDITION true
  #endif
  uint32_t highest_rate = 1;
  LOOP_XYZE_N(i) {
    max_acceleration_steps_per_s2[i] = settings.max_acceleration_mm_per_s2[i] * settings.axis_steps_per_mm[i];
    if (AXIS_CONDITION) NOLESS(highest_rate, max_acceleration_steps_per_s2[i]);
  }
  cutoff_long = 4294967295UL / highest_rate; // 0xFFFFFFFFUL
  #if HAS_LINEAR_E_JERK
    recalculate_max_e_jerk();
  #endif
}

Here is the caller graph for this function:

refresh_positioning()

void Planner::refresh_positioning ( )

static

                                  {
  LOOP_XYZE_N(i) steps_to_mm[i] = 1.0f / settings.axis_steps_per_mm[i];
  set_position_mm(current_position);
  reset_acceleration_rates();
}

Here is the call graph for this function:

set_max_acceleration()

void Planner::set_max_acceleration ( const uint8_t  axis, float  targetValue  )

static

                                                                        {
  #if ENABLED(LIMITED_MAX_ACCEL_EDITING)
    #ifdef MAX_ACCEL_EDIT_VALUES
      constexpr xyze_float_t max_accel_edit = MAX_ACCEL_EDIT_VALUES;
      const xyze_float_t &max_acc_edit_scaled = max_accel_edit;
    #else
      constexpr xyze_float_t max_accel_edit = DEFAULT_MAX_ACCELERATION,
                             max_acc_edit_scaled = max_accel_edit * 2;
    #endif
    limit_and_warn(targetValue, axis, PSTR("Acceleration"), max_acc_edit_scaled);
  #endif
  settings.max_acceleration_mm_per_s2[axis] = targetValue;
 
  // Update steps per s2 to agree with the units per s2 (since they are used in the planner)
  reset_acceleration_rates();
}

Here is the call graph for this function:

set_max_feedrate()

void Planner::set_max_feedrate ( const uint8_t  axis, float  targetValue  )

static

                                                                    {
  #if ENABLED(LIMITED_MAX_FR_EDITING)
    #ifdef MAX_FEEDRATE_EDIT_VALUES
      constexpr xyze_float_t max_fr_edit = MAX_FEEDRATE_EDIT_VALUES;
      const xyze_float_t &max_fr_edit_scaled = max_fr_edit;
    #else
      constexpr xyze_float_t max_fr_edit = DEFAULT_MAX_FEEDRATE,
                             max_fr_edit_scaled = max_fr_edit * 2;
    #endif
    limit_and_warn(targetValue, axis, PSTR("Feedrate"), max_fr_edit_scaled);
  #endif
  settings.max_feedrate_mm_s[axis] = targetValue;
}

Here is the call graph for this function:

set_max_jerk()

void Planner::set_max_jerk ( const AxisEnum  axis, float  targetValue  )

static

                                                                 {
  #if HAS_CLASSIC_JERK
    #if ENABLED(LIMITED_JERK_EDITING)
      constexpr xyze_float_t max_jerk_edit =
        #ifdef MAX_JERK_EDIT_VALUES
          MAX_JERK_EDIT_VALUES
        #else
          { (DEFAULT_XJERK) * 2, (DEFAULT_YJERK) * 2,
            (DEFAULT_ZJERK) * 2, (DEFAULT_EJERK) * 2 }
        #endif
      ;
      limit_and_warn(targetValue, axis, PSTR("Jerk"), max_jerk_edit);
    #endif
    max_jerk[axis] = targetValue;
  #else
    UNUSED(axis); UNUSED(targetValue);
  #endif
}

Here is the call graph for this function:

check_axes_activity()

void Planner::check_axes_activity ( )

static

Maintain fans, paste extruder pressure,

                                  {
 
  #if ANY(DISABLE_X, DISABLE_Y, DISABLE_Z, DISABLE_E)
    xyze_bool_t axis_active = { false };
  #endif
 
  #if FAN_COUNT > 0
    uint8_t tail_fan_speed[FAN_COUNT];
  #endif
 
  #if ENABLED(BARICUDA)
    #if HAS_HEATER_1
      uint8_t tail_valve_pressure;
    #endif
    #if HAS_HEATER_2
      uint8_t tail_e_to_p_pressure;
    #endif
  #endif
 
  if (has_blocks_queued()) {
 
    #if FAN_COUNT > 0 || ENABLED(BARICUDA)
      block_t *block = &block_buffer[block_buffer_tail];
    #endif
 
    #if FAN_COUNT > 0
      FANS_LOOP(i)
        tail_fan_speed[i] = thermalManager.scaledFanSpeed(i, block->fan_speed[i]);
    #endif
 
    #if ENABLED(BARICUDA)
      #if HAS_HEATER_1
        tail_valve_pressure = block->valve_pressure;
      #endif
      #if HAS_HEATER_2
        tail_e_to_p_pressure = block->e_to_p_pressure;
      #endif
    #endif
 
    #if ANY(DISABLE_X, DISABLE_Y, DISABLE_Z, DISABLE_E)
      for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
        block_t *block = &block_buffer[b];
        LOOP_XYZE(i) if (block->steps[i]) axis_active[i] = true;
      }
    #endif
  }
  else {
 
    #if HAS_CUTTER
      cutter.refresh();
    #endif
 
    #if FAN_COUNT > 0
      FANS_LOOP(i)
        tail_fan_speed[i] = thermalManager.scaledFanSpeed(i);
    #endif
 
    #if ENABLED(BARICUDA)
      #if HAS_HEATER_1
        tail_valve_pressure = baricuda_valve_pressure;
      #endif
      #if HAS_HEATER_2
        tail_e_to_p_pressure = baricuda_e_to_p_pressure;
      #endif
    #endif
  }
 
  //
  // Disable inactive axes
  //
  #if ENABLED(DISABLE_X)
    if (!axis_active.x) disable_X();
  #endif
  #if ENABLED(DISABLE_Y)
    if (!axis_active.y) disable_Y();
  #endif
  #if ENABLED(DISABLE_Z)
    if (!axis_active.z) disable_Z();
  #endif
  #if ENABLED(DISABLE_E)
    if (!axis_active.e) disable_e_steppers();
  #endif
 
  //
  // Update Fan speeds
  //
  #if FAN_COUNT > 0
 
    #if FAN_KICKSTART_TIME > 0
      static millis_t fan_kick_end[FAN_COUNT] = { 0 };
      #define KICKSTART_FAN(f)                         \
        if (tail_fan_speed[f]) {                       \
          millis_t ms = millis();                      \
          if (fan_kick_end[f] == 0) {                  \
            fan_kick_end[f] = ms + FAN_KICKSTART_TIME; \
            tail_fan_speed[f] = 255;                   \
          } else if (PENDING(ms, fan_kick_end[f]))     \
            tail_fan_speed[f] = 255;                   \
        } else fan_kick_end[f] = 0
    #else
      #define KICKSTART_FAN(f) NOOP
    #endif
 
    #if FAN_MIN_PWM != 0 || FAN_MAX_PWM != 255
      #define CALC_FAN_SPEED(f) (tail_fan_speed[f] ? map(tail_fan_speed[f], 1, 255, FAN_MIN_PWM, FAN_MAX_PWM) : 0)
    #else
      #define CALC_FAN_SPEED(f) tail_fan_speed[f]
    #endif
 
    #if ENABLED(FAN_SOFT_PWM)
      #define _FAN_SET(F) thermalManager.soft_pwm_amount_fan[F] = CALC_FAN_SPEED(F);
    #elif ENABLED(FAST_PWM_FAN)
      #define _FAN_SET(F) set_pwm_duty(FAN##F##_PIN, CALC_FAN_SPEED(F));
    #else
      #define _FAN_SET(F) analogWrite(pin_t(FAN##F##_PIN), CALC_FAN_SPEED(F));
    #endif
    #define FAN_SET(F) do{ KICKSTART_FAN(F); _FAN_SET(F); }while(0)
 
    #if HAS_FAN0
      FAN_SET(0);
    #endif
    #if HAS_FAN1
      FAN_SET(1);
    #endif
    #if HAS_FAN2
      FAN_SET(2);
    #endif
 
  #endif // FAN_COUNT > 0
 
  #if ENABLED(AUTOTEMP)
    getHighESpeed();
  #endif
 
  #if ENABLED(BARICUDA)
    #if HAS_HEATER_1
      analogWrite(pin_t(HEATER_1_PIN), tail_valve_pressure);
    #endif
    #if HAS_HEATER_2
      analogWrite(pin_t(HEATER_2_PIN), tail_e_to_p_pressure);
    #endif
  #endif
}

Here is the call graph for this function:

Here is the caller graph for this function:

calculate_volumetric_multipliers()

static void Planner::calculate_volumetric_multipliers ( )

static

fade_scaling_factor_for_z()

static FORCE_INLINE float Planner::fade_scaling_factor_for_z ( const float &  )

static

{ return 1; }

Here is the caller graph for this function:

leveling_active_at_z()

static FORCE_INLINE bool Planner::leveling_active_at_z ( const float &  )

static

{ return true; }

Here is the caller graph for this function:

apply_leveling()

void Planner::apply_leveling ( xyz_pos_t &  raw )

static

Apply leveling to transform a cartesian position as it will be given to the planner and steppers.

rx, ry, rz - Cartesian positions in mm Leveled XYZ on completion

                                             {
    if (!leveling_active) return;
 
    #if ABL_PLANAR
 
      xy_pos_t d = raw - level_fulcrum;
      apply_rotation_xyz(bed_level_matrix, d.x, d.y, raw.z);
      raw = d + level_fulcrum;
 
    #elif HAS_MESH
 
      #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
        const float fade_scaling_factor = fade_scaling_factor_for_z(raw.z);
      #else
        constexpr float fade_scaling_factor = 1.0;
      #endif
 
      raw.z += (
        #if ENABLED(MESH_BED_LEVELING)
          mbl.get_z(raw
            #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
              , fade_scaling_factor
            #endif
          )
        #elif ENABLED(AUTO_BED_LEVELING_UBL)
          fade_scaling_factor ? fade_scaling_factor * ubl.get_z_correction(raw) : 0.0
        #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
          fade_scaling_factor ? fade_scaling_factor * bilinear_z_offset(raw) : 0.0
        #endif
      );
 
    #endif
  }

Here is the call graph for this function:

Here is the caller graph for this function:

unapply_leveling()

void Planner::unapply_leveling ( xyz_pos_t &  raw )

static

                                               {
 
    if (leveling_active) {
 
      #if ABL_PLANAR
 
        matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
 
        xy_pos_t d = raw - level_fulcrum;
        apply_rotation_xyz(inverse, d.x, d.y, raw.z);
        raw = d + level_fulcrum;
 
      #elif HAS_MESH
 
        #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
          const float fade_scaling_factor = fade_scaling_factor_for_z(raw.z);
        #else
          constexpr float fade_scaling_factor = 1.0;
        #endif
 
        raw.z -= (
          #if ENABLED(MESH_BED_LEVELING)
            mbl.get_z(raw
              #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
                , fade_scaling_factor
              #endif
            )
          #elif ENABLED(AUTO_BED_LEVELING_UBL)
            fade_scaling_factor ? fade_scaling_factor * ubl.get_z_correction(raw) : 0.0
          #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
            fade_scaling_factor ? fade_scaling_factor * bilinear_z_offset(raw) : 0.0
          #endif
        );
 
      #endif
    }
 
    #if ENABLED(SKEW_CORRECTION)
      unskew(raw);
    #endif
  }

Here is the call graph for this function:

Here is the caller graph for this function:

force_unapply_leveling()

static FORCE_INLINE void Planner::force_unapply_leveling ( xyz_pos_t &  raw )

static

                                                                      {
        leveling_active = true;
        unapply_leveling(raw);
        leveling_active = false;
      }

Here is the call graph for this function:

apply_modifiers()

static FORCE_INLINE void Planner::apply_modifiers ( xyze_pos_t &  pos, bool  leveling = false  )

static

        {
        #if ENABLED(SKEW_CORRECTION)
          skew(pos);
        #endif
        #if HAS_LEVELING
          if (leveling) apply_leveling(pos);
        #endif
        #if ENABLED(FWRETRACT)
          apply_retract(pos);
        #endif
      }

Here is the call graph for this function:

Here is the caller graph for this function:

unapply_modifiers()

static FORCE_INLINE void Planner::unapply_modifiers ( xyze_pos_t &  pos, bool  leveling = false  )

static

        {
        #if ENABLED(FWRETRACT)
          unapply_retract(pos);
        #endif
        #if HAS_LEVELING
          if (leveling) unapply_leveling(pos);
        #endif
        #if ENABLED(SKEW_CORRECTION)
          unskew(pos);
        #endif
      }

Here is the call graph for this function:

Here is the caller graph for this function:

movesplanned()

static FORCE_INLINE uint8_t Planner::movesplanned ( )

static

{ return BLOCK_MOD(block_buffer_head - block_buffer_tail); }

Here is the caller graph for this function:

nonbusy_movesplanned()

static FORCE_INLINE uint8_t Planner::nonbusy_movesplanned ( )

static

{ return BLOCK_MOD(block_buffer_head - block_buffer_nonbusy); }

Here is the caller graph for this function:

clear_block_buffer()

static FORCE_INLINE void Planner::clear_block_buffer ( )

static

{ block_buffer_nonbusy = block_buffer_planned = block_buffer_head = block_buffer_tail = 0; }

Here is the caller graph for this function:

is_full()

static FORCE_INLINE bool Planner::is_full ( )

static

{ return block_buffer_tail == next_block_index(block_buffer_head); }

moves_free()

static FORCE_INLINE uint8_t Planner::moves_free ( )

static

{ return BLOCK_BUFFER_SIZE - 1 - movesplanned(); }

Here is the call graph for this function:

Here is the caller graph for this function:

get_next_free_block()

static FORCE_INLINE block_t* Planner::get_next_free_block ( uint8_t &  next_buffer_head, const uint8_t  count = 1  )

static

Planner::get_next_free_block

• Get the next head indices (passed by reference)
• Wait for the number of spaces to open up in the planner
• Return the first head block

                                                                                                       {
 
      // Wait until there are enough slots free
      while (moves_free() < count) { idle(); }
 
      // Return the first available block
      next_buffer_head = next_block_index(block_buffer_head);
      return &block_buffer[block_buffer_head];
    }

Here is the call graph for this function:

Here is the caller graph for this function:

_buffer_steps()

bool Planner::_buffer_steps ( const xyze_long_t &  target, const xyze_pos_t &  target_float, feedRate_t  fr_mm_s, const uint8_t  extruder, const float &  millimeters = 0.0  )

static

Planner::_buffer_steps

Add a new linear movement to the buffer (in terms of steps).

target - target position in steps units fr_mm_s - (target) speed of the move extruder - target extruder millimeters - the length of the movement, if known

Returns true if movement was buffered, false otherwise

Planner::_buffer_steps

Add a new linear movement to the planner queue (in terms of steps).

target - target position in steps units target_float - target position in direct (mm, degrees) units. optional fr_mm_s - (target) speed of the move extruder - target extruder millimeters - the length of the movement, if known

Returns true if movement was properly queued, false otherwise

  {
 
  // If we are cleaning, do not accept queuing of movements
  if (cleaning_buffer_counter) return false;
 
  // Wait for the next available block
  uint8_t next_buffer_head;
  block_t * const block = get_next_free_block(next_buffer_head);
 
  // Fill the block with the specified movement
  if (!_populate_block(block, false, target
    #if HAS_POSITION_FLOAT
      , target_float
    #endif
    #if IS_KINEMATIC && DISABLED(CLASSIC_JERK)
      , delta_mm_cart
    #endif
    , fr_mm_s, extruder, millimeters
  )) {
    // Movement was not queued, probably because it was too short.
    //  Simply accept that as movement queued and done
    return true;
  }
 
  // If this is the first added movement, reload the delay, otherwise, cancel it.
  if (block_buffer_head == block_buffer_tail) {
    // If it was the first queued block, restart the 1st block delivery delay, to
    // give the planner an opportunity to queue more movements and plan them
    // As there are no queued movements, the Stepper ISR will not touch this
    // variable, so there is no risk setting this here (but it MUST be done
    // before the following line!!)
    delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
  }
 
  // Move buffer head
  block_buffer_head = next_buffer_head;
 
  // Recalculate and optimize trapezoidal speed profiles
  recalculate();
 
  // Movement successfully queued!
  return true;
}

Here is the call graph for this function:

Here is the caller graph for this function:

_populate_block()

bool Planner::_populate_block ( block_t *const  block, bool  split_move, const xyze_long_t &  target, const xyze_pos_t &  target_float, feedRate_t  fr_mm_s, const uint8_t  extruder, const float &  millimeters = 0.0  )

static

Planner::_populate_block

Fills a new linear movement in the block (in terms of steps).

target - target position in steps units fr_mm_s - (target) speed of the move extruder - target extruder millimeters - the length of the movement, if known

Returns true is movement is acceptable, false otherwise

Planner::_populate_block

Fills a new linear movement in the block (in terms of steps).

target - target position in steps units fr_mm_s - (target) speed of the move extruder - target extruder

Returns true is movement is acceptable, false otherwise

This part of the code calculates the total length of the movement. For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS. But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y. So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head. Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.

At this point at least one of the axes has more steps than MIN_STEPS_PER_SEGMENT, ensuring the segment won't get dropped as zero-length. It's important to not apply corrections to blocks that would get dropped!

A correction function is permitted to add steps to an axis, it should never remove steps!

  {
 
  const int32_t da = target.a - position.a,
                db = target.b - position.b,
                dc = target.c - position.c;
 
  #if EXTRUDERS
    int32_t de = target.e - position.e;
  #else
    constexpr int32_t de = 0;
  #endif
 
  /* <-- add a slash to enable
    SERIAL_ECHOLNPAIR("  _populate_block FR:", fr_mm_s,
                      " A:", target.a, " (", da, " steps)"
                      " B:", target.b, " (", db, " steps)"
                      " C:", target.c, " (", dc, " steps)"
                      #if EXTRUDERS
                        " E:", target.e, " (", de, " steps)"
                      #endif
                    );
  //*/
 
  #if EITHER(PREVENT_COLD_EXTRUSION, PREVENT_LENGTHY_EXTRUDE)
    if (de) {
      #if ENABLED(PREVENT_COLD_EXTRUSION)
        if (thermalManager.tooColdToExtrude(extruder)) {
          position.e = target.e; // Behave as if the move really took place, but ignore E part
          #if HAS_POSITION_FLOAT
            position_float.e = target_float.e;
          #endif
          de = 0; // no difference
          SERIAL_ECHO_MSG(MSG_ERR_COLD_EXTRUDE_STOP);
        }
      #endif // PREVENT_COLD_EXTRUSION
      #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
        const float e_steps = ABS(de * e_factor[extruder]);
        const float max_e_steps = settings.axis_steps_per_mm[E_AXIS_N(extruder)] * (EXTRUDE_MAXLENGTH);
        if (e_steps > max_e_steps) {
          #if ENABLED(MIXING_EXTRUDER)
            bool ignore_e = false;
            float collector[MIXING_STEPPERS];
            mixer.refresh_collector(1.0, mixer.get_current_vtool(), collector);
            MIXER_STEPPER_LOOP(e)
              if (e_steps * collector[e] > max_e_steps) { ignore_e = true; break; }
          #else
            constexpr bool ignore_e = true;
          #endif
          if (ignore_e) {
            position.e = target.e; // Behave as if the move really took place, but ignore E part
            #if HAS_POSITION_FLOAT
              position_float.e = target_float.e;
            #endif
            de = 0; // no difference
            SERIAL_ECHO_MSG(MSG_ERR_LONG_EXTRUDE_STOP);
          }
        }
      #endif // PREVENT_LENGTHY_EXTRUDE
    }
  #endif // PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE
 
  // Compute direction bit-mask for this block
  uint8_t dm = 0;
  #if CORE_IS_XY
    if (da < 0) SBI(dm, X_HEAD);                // Save the real Extruder (head) direction in X Axis
    if (db < 0) SBI(dm, Y_HEAD);                // ...and Y
    if (dc < 0) SBI(dm, Z_AXIS);
    if (da + db < 0) SBI(dm, A_AXIS);           // Motor A direction
    if (CORESIGN(da - db) < 0) SBI(dm, B_AXIS); // Motor B direction
  #elif CORE_IS_XZ
    if (da < 0) SBI(dm, X_HEAD);                // Save the real Extruder (head) direction in X Axis
    if (db < 0) SBI(dm, Y_AXIS);
    if (dc < 0) SBI(dm, Z_HEAD);                // ...and Z
    if (da + dc < 0) SBI(dm, A_AXIS);           // Motor A direction
    if (CORESIGN(da - dc) < 0) SBI(dm, C_AXIS); // Motor C direction
  #elif CORE_IS_YZ
    if (da < 0) SBI(dm, X_AXIS);
    if (db < 0) SBI(dm, Y_HEAD);                // Save the real Extruder (head) direction in Y Axis
    if (dc < 0) SBI(dm, Z_HEAD);                // ...and Z
    if (db + dc < 0) SBI(dm, B_AXIS);           // Motor B direction
    if (CORESIGN(db - dc) < 0) SBI(dm, C_AXIS); // Motor C direction
  #else
    if (da < 0) SBI(dm, X_AXIS);
    if (db < 0) SBI(dm, Y_AXIS);
    if (dc < 0) SBI(dm, Z_AXIS);
  #endif
  if (de < 0) SBI(dm, E_AXIS);
 
  #if EXTRUDERS
    const float esteps_float = de * e_factor[extruder];
    const uint32_t esteps = ABS(esteps_float) + 0.5f;
  #else
    constexpr uint32_t esteps = 0;
  #endif
 
  // Clear all flags, including the "busy" bit
  block->flag = 0x00;
 
  // Set direction bits
  block->direction_bits = dm;
 
  // Number of steps for each axis
  // See http://www.corexy.com/theory.html
  #if CORE_IS_XY
    block->steps.set(ABS(da + db), ABS(da - db), ABS(dc));
  #elif CORE_IS_XZ
    block->steps.set(ABS(da + dc), ABS(db), ABS(da - dc));
  #elif CORE_IS_YZ
    block->steps.set(ABS(da), ABS(db + dc), ABS(db - dc));
  #elif IS_SCARA
    block->steps.set(ABS(da), ABS(db), ABS(dc));
  #else
    // default non-h-bot planning
    block->steps.set(ABS(da), ABS(db), ABS(dc));
  #endif
 
  /**
   * This part of the code calculates the total length of the movement.
   * For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS.
   * But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS
   * and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y.
   * So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head.
   * Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
   */
  struct DeltaMM : abce_float_t {
    #if IS_CORE
      xyz_pos_t head;
    #endif
  } delta_mm;
  #if IS_CORE
    #if CORE_IS_XY
      delta_mm.head.x = da * steps_to_mm[A_AXIS];
      delta_mm.head.y = db * steps_to_mm[B_AXIS];
      delta_mm.z      = dc * steps_to_mm[Z_AXIS];
      delta_mm.a      = (da + db) * steps_to_mm[A_AXIS];
      delta_mm.b      = CORESIGN(da - db) * steps_to_mm[B_AXIS];
    #elif CORE_IS_XZ
      delta_mm.head.x = da * steps_to_mm[A_AXIS];
      delta_mm.y      = db * steps_to_mm[Y_AXIS];
      delta_mm.head.z = dc * steps_to_mm[C_AXIS];
      delta_mm.a      = (da + dc) * steps_to_mm[A_AXIS];
      delta_mm.c      = CORESIGN(da - dc) * steps_to_mm[C_AXIS];
    #elif CORE_IS_YZ
      delta_mm.x      = da * steps_to_mm[X_AXIS];
      delta_mm.head.y = db * steps_to_mm[B_AXIS];
      delta_mm.head.z = dc * steps_to_mm[C_AXIS];
      delta_mm.b      = (db + dc) * steps_to_mm[B_AXIS];
      delta_mm.c      = CORESIGN(db - dc) * steps_to_mm[C_AXIS];
    #endif
  #else
    delta_mm.a = da * steps_to_mm[A_AXIS];
    delta_mm.b = db * steps_to_mm[B_AXIS];
    delta_mm.c = dc * steps_to_mm[C_AXIS];
  #endif
 
  #if EXTRUDERS
    delta_mm.e = esteps_float * steps_to_mm[E_AXIS_N(extruder)];
  #endif
 
  if (block->steps.a < MIN_STEPS_PER_SEGMENT && block->steps.b < MIN_STEPS_PER_SEGMENT && block->steps.c < MIN_STEPS_PER_SEGMENT) {
    block->millimeters = (0
      #if EXTRUDERS
        + ABS(delta_mm.e)
      #endif
    );
  }
  else {
    if (millimeters)
      block->millimeters = millimeters;
    else
      block->millimeters = SQRT(
        #if CORE_IS_XY
          sq(delta_mm.head.x) + sq(delta_mm.head.y) + sq(delta_mm.z)
        #elif CORE_IS_XZ
          sq(delta_mm.head.x) + sq(delta_mm.y) + sq(delta_mm.head.z)
        #elif CORE_IS_YZ
          sq(delta_mm.x) + sq(delta_mm.head.y) + sq(delta_mm.head.z)
        #else
          sq(delta_mm.x) + sq(delta_mm.y) + sq(delta_mm.z)
        #endif
      );
 
    /**
     * At this point at least one of the axes has more steps than
     * MIN_STEPS_PER_SEGMENT, ensuring the segment won't get dropped as
     * zero-length. It's important to not apply corrections
     * to blocks that would get dropped!
     *
     * A correction function is permitted to add steps to an axis, it
     * should *never* remove steps!
     */
    #if ENABLED(BACKLASH_COMPENSATION)
      backlash.add_correction_steps(da, db, dc, dm, block);
    #endif
  }
 
  #if EXTRUDERS
    block->steps.e = esteps;
  #endif
 
  block->step_event_count = _MAX(block->steps.a, block->steps.b, block->steps.c, esteps);
 
  // Bail if this is a zero-length block
  if (block->step_event_count < MIN_STEPS_PER_SEGMENT) return false;
 
  #if ENABLED(MIXING_EXTRUDER)
    MIXER_POPULATE_BLOCK();
  #endif
 
  #if HAS_CUTTER
    block->cutter_power = cutter.power;
  #endif
 
  #if FAN_COUNT > 0
    FANS_LOOP(i) block->fan_speed[i] = thermalManager.fan_speed[i];
  #endif
 
  #if ENABLED(BARICUDA)
    block->valve_pressure = baricuda_valve_pressure;
    block->e_to_p_pressure = baricuda_e_to_p_pressure;
  #endif
 
  #if EXTRUDERS > 1
    block->extruder = extruder;
  #endif
 
  #if ENABLED(AUTO_POWER_CONTROL)
    if (block->steps.x || block->steps.y || block->steps.z)
      powerManager.power_on();
  #endif
 
  // Enable active axes
  #if CORE_IS_XY
    if (block->steps.a || block->steps.b) {
      enable_X();
      enable_Y();
    }
    #if DISABLED(Z_LATE_ENABLE)
      if (block->steps.z) enable_Z();
    #endif
  #elif CORE_IS_XZ
    if (block->steps.a || block->steps.c) {
      enable_X();
      enable_Z();
    }
    if (block->steps.y) enable_Y();
  #elif CORE_IS_YZ
    if (block->steps.b || block->steps.c) {
      enable_Y();
      enable_Z();
    }
    if (block->steps.x) enable_X();
  #else
    if (block->steps.x) enable_X();
    if (block->steps.y) enable_Y();
    #if DISABLED(Z_LATE_ENABLE)
      if (block->steps.z) enable_Z();
    #endif
  #endif
 
  // Enable extruder(s)
  #if EXTRUDERS
    if (esteps) {
      #if ENABLED(AUTO_POWER_CONTROL)
        powerManager.power_on();
      #endif
 
      #if ENABLED(DISABLE_INACTIVE_EXTRUDER) // Enable only the selected extruder
 
        #define DISABLE_IDLE_E(N) if (!g_uc_extruder_last_move[N]) disable_E##N();
 
        for (uint8_t i = 0; i < EXTRUDERS; i++)
          if (g_uc_extruder_last_move[i] > 0) g_uc_extruder_last_move[i]--;
 
        switch (extruder) {
          case 0:
            #if EXTRUDERS > 1
              DISABLE_IDLE_E(1);
              #if EXTRUDERS > 2
                DISABLE_IDLE_E(2);
                #if EXTRUDERS > 3
                  DISABLE_IDLE_E(3);
                  #if EXTRUDERS > 4
                    DISABLE_IDLE_E(4);
                    #if EXTRUDERS > 5
                      DISABLE_IDLE_E(5);
                    #endif // EXTRUDERS > 5
                  #endif // EXTRUDERS > 4
                #endif // EXTRUDERS > 3
              #endif // EXTRUDERS > 2
            #endif // EXTRUDERS > 1
            enable_E0();
            g_uc_extruder_last_move[0] = (BLOCK_BUFFER_SIZE) * 2;
            #if HAS_DUPLICATION_MODE
              if (extruder_duplication_enabled) {
                enable_E1();
                g_uc_extruder_last_move[1] = (BLOCK_BUFFER_SIZE) * 2;
              }
            #endif
          break;
          #if EXTRUDERS > 1
            case 1:
              DISABLE_IDLE_E(0);
              #if EXTRUDERS > 2
                DISABLE_IDLE_E(2);
                #if EXTRUDERS > 3
                  DISABLE_IDLE_E(3);
                  #if EXTRUDERS > 4
                    DISABLE_IDLE_E(4);
                    #if EXTRUDERS > 5
                      DISABLE_IDLE_E(5);
                    #endif // EXTRUDERS > 5
                  #endif // EXTRUDERS > 4
                #endif // EXTRUDERS > 3
              #endif // EXTRUDERS > 2
              enable_E1();
              g_uc_extruder_last_move[1] = (BLOCK_BUFFER_SIZE) * 2;
            break;
            #if EXTRUDERS > 2
              case 2:
                DISABLE_IDLE_E(0);
                DISABLE_IDLE_E(1);
                #if EXTRUDERS > 3
                  DISABLE_IDLE_E(3);
                  #if EXTRUDERS > 4
                    DISABLE_IDLE_E(4);
                    #if EXTRUDERS > 5
                      DISABLE_IDLE_E(5);
                    #endif
                  #endif
                #endif
                enable_E2();
                g_uc_extruder_last_move[2] = (BLOCK_BUFFER_SIZE) * 2;
              break;
              #if EXTRUDERS > 3
                case 3:
                  DISABLE_IDLE_E(0);
                  DISABLE_IDLE_E(1);
                  DISABLE_IDLE_E(2);
                  #if EXTRUDERS > 4
                    DISABLE_IDLE_E(4);
                    #if EXTRUDERS > 5
                      DISABLE_IDLE_E(5);
                    #endif
                  #endif
                  enable_E3();
                  g_uc_extruder_last_move[3] = (BLOCK_BUFFER_SIZE) * 2;
                break;
                #if EXTRUDERS > 4
                  case 4:
                    DISABLE_IDLE_E(0);
                    DISABLE_IDLE_E(1);
                    DISABLE_IDLE_E(2);
                    DISABLE_IDLE_E(3);
                    #if EXTRUDERS > 5
                      DISABLE_IDLE_E(5);
                    #endif
                    enable_E4();
                    g_uc_extruder_last_move[4] = (BLOCK_BUFFER_SIZE) * 2;
                  break;
                  #if EXTRUDERS > 5
                    case 5:
                      DISABLE_IDLE_E(0);
                      DISABLE_IDLE_E(1);
                      DISABLE_IDLE_E(2);
                      DISABLE_IDLE_E(3);
                      DISABLE_IDLE_E(4);
                      enable_E5();
                      g_uc_extruder_last_move[5] = (BLOCK_BUFFER_SIZE) * 2;
                    break;
                  #endif // EXTRUDERS > 5
                #endif // EXTRUDERS > 4
              #endif // EXTRUDERS > 3
            #endif // EXTRUDERS > 2
          #endif // EXTRUDERS > 1
        }
      #else
        enable_E0();
        enable_E1();
        enable_E2();
        enable_E3();
        enable_E4();
        enable_E5();
      #endif
    }
  #endif // EXTRUDERS
 
  if (esteps)
    NOLESS(fr_mm_s, settings.min_feedrate_mm_s);
  else
    NOLESS(fr_mm_s, settings.min_travel_feedrate_mm_s);
 
  const float inverse_millimeters = 1.0f / block->millimeters;  // Inverse millimeters to remove multiple divides
 
  // Calculate inverse time for this move. No divide by zero due to previous checks.
  // Example: At 120mm/s a 60mm move takes 0.5s. So this will give 2.0.
  float inverse_secs = fr_mm_s * inverse_millimeters;
 
  // Get the number of non busy movements in queue (non busy means that they can be altered)
  const uint8_t moves_queued = nonbusy_movesplanned();
 
  // Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
  #if EITHER(SLOWDOWN, ULTRA_LCD) || defined(XY_FREQUENCY_LIMIT)
    // Segment time im micro seconds
    uint32_t segment_time_us = LROUND(1000000.0f / inverse_secs);
  #endif
 
  #if ENABLED(SLOWDOWN)
    if (WITHIN(moves_queued, 2, (BLOCK_BUFFER_SIZE) / 2 - 1)) {
      if (segment_time_us < settings.min_segment_time_us) {
        // buffer is draining, add extra time.  The amount of time added increases if the buffer is still emptied more.
        const uint32_t nst = segment_time_us + LROUND(2 * (settings.min_segment_time_us - segment_time_us) / moves_queued);
        inverse_secs = 1000000.0f / nst;
        #if defined(XY_FREQUENCY_LIMIT) || HAS_SPI_LCD
          segment_time_us = nst;
        #endif
      }
    }
  #endif
 
  #if HAS_SPI_LCD
    // Protect the access to the position.
    const bool was_enabled = STEPPER_ISR_ENABLED();
    if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
 
    block_buffer_runtime_us += segment_time_us;
    block->segment_time_us = segment_time_us;
 
    if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
  #endif
 
  block->nominal_speed_sqr = sq(block->millimeters * inverse_secs);   //   (mm/sec)^2 Always > 0
  block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0
 
  #if ENABLED(FILAMENT_WIDTH_SENSOR)
    if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM)   // Only for extruder with filament sensor
      filwidth.advance_e(delta_mm.e);
  #endif
 
  // Calculate and limit speed in mm/sec for each axis
  xyze_float_t current_speed;
  float speed_factor = 1.0f; // factor <1 decreases speed
  LOOP_XYZE(i) {
    #if BOTH(MIXING_EXTRUDER, RETRACT_SYNC_MIXING)
      // In worst case, only one extruder running, no change is needed.
      // In best case, all extruders run the same amount, we can divide by MIXING_STEPPERS
      float delta_mm_i = 0;
      if (i == E_AXIS && mixer.get_current_vtool() == MIXER_AUTORETRACT_TOOL)
        delta_mm_i = delta_mm.e / MIXING_STEPPERS;
      else
        delta_mm_i = delta_mm.e;
    #else
      const float delta_mm_i = delta_mm[i];
    #endif
    const feedRate_t cs = ABS(current_speed[i] = delta_mm_i * inverse_secs);
    #if ENABLED(DISTINCT_E_FACTORS)
      if (i == E_AXIS) i += extruder;
    #endif
    if (cs > settings.max_feedrate_mm_s[i]) NOMORE(speed_factor, settings.max_feedrate_mm_s[i] / cs);
  }
 
  // Max segment time in µs.
  #ifdef XY_FREQUENCY_LIMIT
 
    // Check and limit the xy direction change frequency
    const unsigned char direction_change = block->direction_bits ^ old_direction_bits;
    old_direction_bits = block->direction_bits;
    segment_time_us = LROUND((float)segment_time_us / speed_factor);
 
    uint32_t xs0 = axis_segment_time_us[0].x,
             xs1 = axis_segment_time_us[1].x,
             xs2 = axis_segment_time_us[2].x,
             ys0 = axis_segment_time_us[0].y,
             ys1 = axis_segment_time_us[1].y,
             ys2 = axis_segment_time_us[2].y;
 
    if (TEST(direction_change, X_AXIS)) {
      xs2 = axis_segment_time_us[2].x = xs1;
      xs1 = axis_segment_time_us[1].x = xs0;
      xs0 = 0;
    }
    xs0 = axis_segment_time_us[0].x = xs0 + segment_time_us;
 
    if (TEST(direction_change, Y_AXIS)) {
      ys2 = axis_segment_time_us[2].y = axis_segment_time_us[1].y;
      ys1 = axis_segment_time_us[1].y = axis_segment_time_us[0].y;
      ys0 = 0;
    }
    ys0 = axis_segment_time_us[0].y = ys0 + segment_time_us;
 
    const uint32_t max_x_segment_time = _MAX(xs0, xs1, xs2),
                   max_y_segment_time = _MAX(ys0, ys1, ys2),
                   min_xy_segment_time = _MIN(max_x_segment_time, max_y_segment_time);
    if (min_xy_segment_time < MAX_FREQ_TIME_US) {
      const float low_sf = speed_factor * min_xy_segment_time / (MAX_FREQ_TIME_US);
      NOMORE(speed_factor, low_sf);
    }
  #endif // XY_FREQUENCY_LIMIT
 
  // Correct the speed
  if (speed_factor < 1.0f) {
    current_speed *= speed_factor;
    block->nominal_rate *= speed_factor;
    block->nominal_speed_sqr = block->nominal_speed_sqr * sq(speed_factor);
  }
 
  // Compute and limit the acceleration rate for the trapezoid generator.
  const float steps_per_mm = block->step_event_count * inverse_millimeters;
  uint32_t accel;
  if (!block->steps.a && !block->steps.b && !block->steps.c) {
    // convert to: acceleration steps/sec^2
    accel = CEIL(settings.retract_acceleration * steps_per_mm);
    #if ENABLED(LIN_ADVANCE)
      block->use_advance_lead = false;
    #endif
  }
  else {
    #define LIMIT_ACCEL_LONG(AXIS,INDX) do{ \
      if (block->steps[AXIS] && max_acceleration_steps_per_s2[AXIS+INDX] < accel) { \
        const uint32_t comp = max_acceleration_steps_per_s2[AXIS+INDX] * block->step_event_count; \
        if (accel * block->steps[AXIS] > comp) accel = comp / block->steps[AXIS]; \
      } \
    }while(0)
 
    #define LIMIT_ACCEL_FLOAT(AXIS,INDX) do{ \
      if (block->steps[AXIS] && max_acceleration_steps_per_s2[AXIS+INDX] < accel) { \
        const float comp = (float)max_acceleration_steps_per_s2[AXIS+INDX] * (float)block->step_event_count; \
        if ((float)accel * (float)block->steps[AXIS] > comp) accel = comp / (float)block->steps[AXIS]; \
      } \
    }while(0)
 
    // Start with print or travel acceleration
    accel = CEIL((esteps ? settings.acceleration : settings.travel_acceleration) * steps_per_mm);
 
    #if ENABLED(LIN_ADVANCE)
 
      #if DISABLED(CLASSIC_JERK)
        #if ENABLED(DISTINCT_E_FACTORS)
          #define MAX_E_JERK max_e_jerk[extruder]
        #else
          #define MAX_E_JERK max_e_jerk
        #endif
      #else
        #define MAX_E_JERK max_jerk.e
      #endif
 
      /**
       *
       * Use LIN_ADVANCE for blocks if all these are true:
       *
       * esteps             : This is a print move, because we checked for A, B, C steps before.
       *
       * extruder_advance_K[active_extruder] : There is an advance factor set for this extruder.
       *
       * de > 0             : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves)
       */
      block->use_advance_lead =  esteps
                              && extruder_advance_K[active_extruder]
                              && de > 0;
 
      if (block->use_advance_lead) {
        block->e_D_ratio = (target_float.e - position_float.e) /
          #if IS_KINEMATIC
            block->millimeters
          #else
            SQRT(sq(target_float.x - position_float.x)
               + sq(target_float.y - position_float.y)
               + sq(target_float.z - position_float.z))
          #endif
        ;
 
        // Check for unusual high e_D ratio to detect if a retract move was combined with the last print move due to min. steps per segment. Never execute this with advance!
        // This assumes no one will use a retract length of 0mm < retr_length < ~0.2mm and no one will print 100mm wide lines using 3mm filament or 35mm wide lines using 1.75mm filament.
        if (block->e_D_ratio > 3.0f)
          block->use_advance_lead = false;
        else {
          const uint32_t max_accel_steps_per_s2 = MAX_E_JERK / (extruder_advance_K[active_extruder] * block->e_D_ratio) * steps_per_mm;
          #if ENABLED(LA_DEBUG)
            if (accel > max_accel_steps_per_s2) SERIAL_ECHOLNPGM("Acceleration limited.");
          #endif
          NOMORE(accel, max_accel_steps_per_s2);
        }
      }
    #endif
 
    #if ENABLED(DISTINCT_E_FACTORS)
      #define ACCEL_IDX extruder
    #else
      #define ACCEL_IDX 0
    #endif
 
    // Limit acceleration per axis
    if (block->step_event_count <= cutoff_long) {
      LIMIT_ACCEL_LONG(A_AXIS, 0);
      LIMIT_ACCEL_LONG(B_AXIS, 0);
      LIMIT_ACCEL_LONG(C_AXIS, 0);
      LIMIT_ACCEL_LONG(E_AXIS, ACCEL_IDX);
    }
    else {
      LIMIT_ACCEL_FLOAT(A_AXIS, 0);
      LIMIT_ACCEL_FLOAT(B_AXIS, 0);
      LIMIT_ACCEL_FLOAT(C_AXIS, 0);
      LIMIT_ACCEL_FLOAT(E_AXIS, ACCEL_IDX);
    }
  }
  block->acceleration_steps_per_s2 = accel;
  block->acceleration = accel / steps_per_mm;
  #if DISABLED(S_CURVE_ACCELERATION)
    block->acceleration_rate = (uint32_t)(accel * (4096.0f * 4096.0f / (STEPPER_TIMER_RATE)));
  #endif
  #if ENABLED(LIN_ADVANCE)
    if (block->use_advance_lead) {
      block->advance_speed = (STEPPER_TIMER_RATE) / (extruder_advance_K[active_extruder] * block->e_D_ratio * block->acceleration * settings.axis_steps_per_mm[E_AXIS_N(extruder)]);
      #if ENABLED(LA_DEBUG)
        if (extruder_advance_K[active_extruder] * block->e_D_ratio * block->acceleration * 2 < SQRT(block->nominal_speed_sqr) * block->e_D_ratio)
          SERIAL_ECHOLNPGM("More than 2 steps per eISR loop executed.");
        if (block->advance_speed < 200)
          SERIAL_ECHOLNPGM("eISR running at > 10kHz.");
      #endif
    }
  #endif
 
  float vmax_junction_sqr; // Initial limit on the segment entry velocity (mm/s)^2
 
  #if DISABLED(CLASSIC_JERK)
    /**
     * Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
     * Let a circle be tangent to both previous and current path line segments, where the junction
     * deviation is defined as the distance from the junction to the closest edge of the circle,
     * colinear with the circle center. The circular segment joining the two paths represents the
     * path of centripetal acceleration. Solve for max velocity based on max acceleration about the
     * radius of the circle, defined indirectly by junction deviation. This may be also viewed as
     * path width or max_jerk in the previous Grbl version. This approach does not actually deviate
     * from path, but used as a robust way to compute cornering speeds, as it takes into account the
     * nonlinearities of both the junction angle and junction velocity.
     *
     * NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path
     * mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact
     * stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here
     * is exactly the same. Instead of motioning all the way to junction point, the machine will
     * just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform
     * a continuous mode path, but ARM-based microcontrollers most certainly do.
     *
     * NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be
     * changed dynamically during operation nor can the line move geometry. This must be kept in
     * memory in the event of a feedrate override changing the nominal speeds of blocks, which can
     * change the overall maximum entry speed conditions of all blocks.
     *
     * #######
     * https://github.com/MarlinFirmware/Marlin/issues/10341#issuecomment-388191754
     *
     * hoffbaked: on May 10 2018 tuned and improved the GRBL algorithm for Marlin:
          Okay! It seems to be working good. I somewhat arbitrarily cut it off at 1mm
          on then on anything with less sides than an octagon. With this, and the
          reverse pass actually recalculating things, a corner acceleration value
          of 1000 junction deviation of .05 are pretty reasonable. If the cycles
          can be spared, a better acos could be used. For all I know, it may be
          already calculated in a different place. */
 
    // Unit vector of previous path line segment
    static xyze_float_t prev_unit_vec;
 
    xyze_float_t unit_vec =
      #if IS_KINEMATIC && DISABLED(CLASSIC_JERK)
        delta_mm_cart
      #else
        { delta_mm.x, delta_mm.y, delta_mm.z, delta_mm.e }
      #endif
    ;
    unit_vec *= inverse_millimeters;
 
    #if IS_CORE && DISABLED(CLASSIC_JERK)
      /**
       * On CoreXY the length of the vector [A,B] is SQRT(2) times the length of the head movement vector [X,Y].
       * So taking Z and E into account, we cannot scale to a unit vector with "inverse_millimeters".
       * => normalize the complete junction vector
       */
      normalize_junction_vector(unit_vec);
    #endif
 
    // 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)) {
      // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
      // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
      float junction_cos_theta = (-prev_unit_vec.x * unit_vec.x) + (-prev_unit_vec.y * unit_vec.y)
                               + (-prev_unit_vec.z * unit_vec.z) + (-prev_unit_vec.e * unit_vec.e);
 
      // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
      if (junction_cos_theta > 0.999999f) {
        // For a 0 degree acute junction, just set minimum junction speed.
        vmax_junction_sqr = sq(float(MINIMUM_PLANNER_SPEED));
      }
      else {
        NOLESS(junction_cos_theta, -0.999999f); // Check for numerical round-off to avoid divide by zero.
 
        // Convert delta vector to unit vector
        xyze_float_t junction_unit_vec = unit_vec - prev_unit_vec;
        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.5f * (1.0f - junction_cos_theta)); // Trig half angle identity. Always positive.
 
        vmax_junction_sqr = (junction_acceleration * junction_deviation_mm * sin_theta_d2) / (1.0f - sin_theta_d2);
        if (block->millimeters < 1) {
 
          // Fast acos approximation, minus the error bar to be safe
          const float junction_theta = (RADIANS(-40) * sq(junction_cos_theta) - RADIANS(50)) * junction_cos_theta + RADIANS(90) - 0.18f;
 
          // 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;
            NOMORE(vmax_junction_sqr, limit_sqr);
          }
        }
      }
 
      // Get the lowest speed
      vmax_junction_sqr = _MIN(vmax_junction_sqr, block->nominal_speed_sqr, previous_nominal_speed_sqr);
    }
    else // Init entry speed to zero. Assume it starts from rest. Planner will correct this later.
      vmax_junction_sqr = 0;
 
    prev_unit_vec = unit_vec;
 
  #endif
 
  #ifdef USE_CACHED_SQRT
    #define CACHED_SQRT(N, V) \
      static float saved_V, N; \
      if (V != saved_V) { N = SQRT(V); saved_V = V; }
  #else
    #define CACHED_SQRT(N, V) const float N = SQRT(V)
  #endif
 
  #if HAS_CLASSIC_JERK
 
    /**
     * Adapted from Průša MKS firmware
     * https://github.com/prusa3d/Prusa-Firmware
     */
    CACHED_SQRT(nominal_speed, block->nominal_speed_sqr);
 
    // Exit speed limited by a jerk to full halt of a previous last segment
    static float previous_safe_speed;
 
    // Start with a safe speed (from which the machine may halt to stop immediately).
    float safe_speed = nominal_speed;
 
    uint8_t limited = 0;
    #if HAS_LINEAR_E_JERK
      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
                  maxj = max_jerk[i];             // mj : The max jerk setting for this axis
      if (jerk > maxj) {                          // cs > mj : New current speed too fast?
        if (limited) {                            // limited already?
          const float mjerk = nominal_speed * maxj; // ns*mj
          if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk; // ns*mj/cs
        }
        else {
          safe_speed *= maxj / jerk;              // Initial limit: ns*mj/cs
          ++limited;                              // Initially limited
        }
      }
    }
 
    float vmax_junction;
    if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {
      // 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,
      // then the machine is not coasting anymore and the safe entry / exit velocities shall be used.
 
      // Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
      float v_factor = 1;
      limited = 0;
 
      // The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum.
      // Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
      CACHED_SQRT(previous_nominal_speed, previous_nominal_speed_sqr);
 
      vmax_junction = _MIN(nominal_speed, previous_nominal_speed);
 
      // Now limit the jerk in all axes.
      const float smaller_speed_factor = vmax_junction / previous_nominal_speed;
      #if HAS_LINEAR_E_JERK
        LOOP_XYZ(axis)
      #else
        LOOP_XYZE(axis)
      #endif
      {
        // Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop.
        float v_exit = previous_speed[axis] * smaller_speed_factor,
              v_entry = current_speed[axis];
        if (limited) {
          v_exit *= v_factor;
          v_entry *= v_factor;
        }
 
        // Calculate jerk depending on whether the axis is coasting in the same direction or reversing.
        const float jerk = (v_exit > v_entry)
            ? //                                  coasting             axis reversal
              ( (v_entry > 0 || v_exit < 0) ? (v_exit - v_entry) : _MAX(v_exit, -v_entry) )
            : // v_exit <= v_entry                coasting             axis reversal
              ( (v_entry < 0 || v_exit > 0) ? (v_entry - v_exit) : _MAX(-v_exit, v_entry) );
 
        if (jerk > max_jerk[axis]) {
          v_factor *= max_jerk[axis] / jerk;
          ++limited;
        }
      }
      if (limited) vmax_junction *= v_factor;
      // Now the transition velocity is known, which maximizes the shared exit / entry velocity while
      // respecting the jerk factors, it may be possible, that applying separate safe exit / entry velocities will achieve faster prints.
      const float vmax_junction_threshold = vmax_junction * 0.99f;
      if (previous_safe_speed > vmax_junction_threshold && safe_speed > vmax_junction_threshold)
        vmax_junction = safe_speed;
    }
    else
      vmax_junction = safe_speed;
 
    previous_safe_speed = safe_speed;
 
    #if DISABLED(CLASSIC_JERK)
      vmax_junction_sqr = _MIN(vmax_junction_sqr, sq(vmax_junction));
    #else
      vmax_junction_sqr = sq(vmax_junction);
    #endif
 
  #endif // Classic Jerk Limiting
 
  // Max entry speed of this block equals the max exit speed of the previous block.
  block->max_entry_speed_sqr = vmax_junction_sqr;
 
  // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
  const float v_allowable_sqr = max_allowable_speed_sqr(-block->acceleration, sq(float(MINIMUM_PLANNER_SPEED)), block->millimeters);
 
  // If we are trying to add a split block, start with the
  // max. allowed speed to avoid an interrupted first move.
  block->entry_speed_sqr = !split_move ? sq(float(MINIMUM_PLANNER_SPEED)) : _MIN(vmax_junction_sqr, v_allowable_sqr);
 
  // Initialize planner efficiency flags
  // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
  // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
  // the current block and next block junction speeds are guaranteed to always be at their maximum
  // junction speeds in deceleration and acceleration, respectively. This is due to how the current
  // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
  // the reverse and forward planners, the corresponding block junction speed will always be at the
  // the maximum junction speed and may always be ignored for any speed reduction checks.
  block->flag |= block->nominal_speed_sqr <= v_allowable_sqr ? BLOCK_FLAG_RECALCULATE | BLOCK_FLAG_NOMINAL_LENGTH : BLOCK_FLAG_RECALCULATE;
 
  // Update previous path unit_vector and nominal speed
  previous_speed = current_speed;
  previous_nominal_speed_sqr = block->nominal_speed_sqr;
 
  // Update the position
  position = target;
  #if HAS_POSITION_FLOAT
    position_float = target_float;
  #endif
 
  #if ENABLED(GRADIENT_MIX)
    mixer.gradient_control(target_float.z);
  #endif
 
  #if ENABLED(POWER_LOSS_RECOVERY)
    block->sdpos = recovery.command_sdpos();
  #endif
 
  // Movement was accepted
  return true;
} // _populate_block()

Here is the call graph for this function:

Here is the caller graph for this function:

buffer_sync_block()

void Planner::buffer_sync_block ( )

static

Planner::buffer_sync_block Add a block to the buffer that just updates the position

                                {
  // Wait for the next available block
  uint8_t next_buffer_head;
  block_t * const block = get_next_free_block(next_buffer_head);
 
  // Clear block
  memset(block, 0, sizeof(block_t));
 
  block->flag = BLOCK_FLAG_SYNC_POSITION;
 
  block->position = position;
 
  // If this is the first added movement, reload the delay, otherwise, cancel it.
  if (block_buffer_head == block_buffer_tail) {
    // If it was the first queued block, restart the 1st block delivery delay, to
    // give the planner an opportunity to queue more movements and plan them
    // As there are no queued movements, the Stepper ISR will not touch this
    // variable, so there is no risk setting this here (but it MUST be done
    // before the following line!!)
    delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
  }
 
  block_buffer_head = next_buffer_head;
 
  stepper.wake_up();
} // buffer_sync_block()

Here is the call graph for this function:

Here is the caller graph for this function:

buffer_segment() [1/2]

bool Planner::buffer_segment ( const float &  a, const float &  b, const float &  c, const float &  e, const feedRate_t &  fr_mm_s, const uint8_t  extruder, const float &  millimeters = 0.0  )

static

Planner::buffer_segment

Add a new linear movement to the buffer in axis units.

Leveling and kinematics should be applied ahead of calling this.

a,b,c,e - target positions in mm and/or degrees fr_mm_s - (target) speed of the move extruder - target extruder millimeters - the length of the movement, if known

  {
 
  // If we are cleaning, do not accept queuing of movements
  if (cleaning_buffer_counter) return false;
 
  // When changing extruders recalculate steps corresponding to the E position
  #if ENABLED(DISTINCT_E_FACTORS)
    if (last_extruder != extruder && settings.axis_steps_per_mm[E_AXIS_N(extruder)] != settings.axis_steps_per_mm[E_AXIS_N(last_extruder)]) {
      position.e = LROUND(position.e * settings.axis_steps_per_mm[E_AXIS_N(extruder)] * steps_to_mm[E_AXIS_N(last_extruder)]);
      last_extruder = extruder;
    }
  #endif
 
  // The target position of the tool in absolute steps
  // Calculate target position in absolute steps
  const abce_long_t target = {
    int32_t(LROUND(a * settings.axis_steps_per_mm[A_AXIS])),
    int32_t(LROUND(b * settings.axis_steps_per_mm[B_AXIS])),
    int32_t(LROUND(c * settings.axis_steps_per_mm[C_AXIS])),
    int32_t(LROUND(e * settings.axis_steps_per_mm[E_AXIS_N(extruder)]))
  };
 
  #if HAS_POSITION_FLOAT
    const xyze_pos_t target_float = { a, b, c, e };
  #endif
 
  // DRYRUN prevents E moves from taking place
  if (DEBUGGING(DRYRUN)) {
    position.e = target.e;
    #if HAS_POSITION_FLOAT
      position_float.e = e;
    #endif
  }
 
  /* <-- add a slash to enable
    SERIAL_ECHOPAIR("  buffer_segment FR:", fr_mm_s);
    #if IS_KINEMATIC
      SERIAL_ECHOPAIR(" A:", a);
      SERIAL_ECHOPAIR(" (", position.a);
      SERIAL_ECHOPAIR("->", target.a);
      SERIAL_ECHOPAIR(") B:", b);
    #else
      SERIAL_ECHOPAIR(" X:", a);
      SERIAL_ECHOPAIR(" (", position.x);
      SERIAL_ECHOPAIR("->", target.x);
      SERIAL_ECHOPAIR(") Y:", b);
    #endif
    SERIAL_ECHOPAIR(" (", position.y);
    SERIAL_ECHOPAIR("->", target.y);
    #if ENABLED(DELTA)
      SERIAL_ECHOPAIR(") C:", c);
    #else
      SERIAL_ECHOPAIR(") Z:", c);
    #endif
    SERIAL_ECHOPAIR(" (", position.z);
    SERIAL_ECHOPAIR("->", target.z);
    SERIAL_ECHOPAIR(") E:", e);
    SERIAL_ECHOPAIR(" (", position.e);
    SERIAL_ECHOPAIR("->", target.e);
    SERIAL_ECHOLNPGM(")");
  //*/
 
  // Queue the movement
  if (
    !_buffer_steps(target
      #if HAS_POSITION_FLOAT
        , target_float
      #endif
      #if IS_KINEMATIC && DISABLED(CLASSIC_JERK)
        , delta_mm_cart
      #endif
      , fr_mm_s, extruder, millimeters
    )
  ) return false;
 
  stepper.wake_up();
  return true;
} // buffer_segment()

Here is the call graph for this function:

Here is the caller graph for this function:

buffer_segment() [2/2]

static FORCE_INLINE bool Planner::buffer_segment ( abce_pos_t &  abce, const feedRate_t &  fr_mm_s, const uint8_t  extruder, const float &  millimeters = 0.0  )

static

      {
      return buffer_segment(abce.a, abce.b, abce.c, abce.e
        #if IS_KINEMATIC && DISABLED(CLASSIC_JERK)
          , delta_mm_cart
        #endif
        , fr_mm_s, extruder, millimeters);
    }

Here is the call graph for this function:

buffer_line() [1/2]

bool Planner::buffer_line ( const float &  rx, const float &  ry, const float &  rz, const float &  e, const feedRate_t &  fr_mm_s, const uint8_t  extruder, const float  millimeters = 0.0  )

static

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)

  {
  xyze_pos_t machine = { rx, ry, rz, e };
  #if HAS_POSITION_MODIFIERS
    apply_modifiers(machine);
  #endif
 
  #if IS_KINEMATIC
 
    #if DISABLED(CLASSIC_JERK)
      const xyze_pos_t delta_mm_cart = {
        rx - position_cart.x, ry - position_cart.y,
        rz - position_cart.z, e  - position_cart.e
      };
    #else
      const xyz_pos_t delta_mm_cart = { rx - position_cart.x, ry - position_cart.y, rz - position_cart.z };
    #endif
 
    float mm = millimeters;
    if (mm == 0.0)
      mm = (delta_mm_cart.x != 0.0 || delta_mm_cart.y != 0.0) ? delta_mm_cart.magnitude() : ABS(delta_mm_cart.z);
 
    inverse_kinematics(machine);
 
    #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 ?: fr_mm_s / mm;
      const feedRate_t feedrate = HYPOT(delta.a - position_float.a, delta.b - position_float.b) * duration_recip;
    #else
      const feedRate_t feedrate = fr_mm_s;
    #endif
    if (buffer_segment(delta.a, delta.b, delta.c, machine.e
      #if DISABLED(CLASSIC_JERK)
        , delta_mm_cart
      #endif
      , feedrate, extruder, mm
    )) {
      position_cart.set(rx, ry, rz, e);
      return true;
    }
    else
      return false;
  #else
    return buffer_segment(machine, fr_mm_s, extruder, millimeters);
  #endif
} // buffer_line()

Here is the call graph for this function:

Here is the caller graph for this function:

buffer_line() [2/2]

static FORCE_INLINE bool Planner::buffer_line ( const xyze_pos_t &  cart, const feedRate_t &  fr_mm_s, const uint8_t  extruder, const float  millimeters = 0.0  )

static

      {
      return buffer_line(cart.x, cart.y, cart.z, cart.e, fr_mm_s, extruder, millimeters
        #if ENABLED(SCARA_FEEDRATE_SCALING)
          , inv_duration
        #endif
      );
    }

Here is the call graph for this function:

set_position_mm() [1/2]

void Planner::set_position_mm ( const float &  rx, const float &  ry, const float &  rz, const float &  e  )

static

Set the planner.position and individual stepper positions. Used by G92, G28, G29, and other procedures.

The supplied position is in the cartesian coordinate space and is translated in to machine space as needed. Modifiers such as leveling and skew are also applied.

Multiplies by axis_steps_per_mm[] and does necessary conversion for COREXY / COREXZ / COREYZ to set the corresponding stepper positions.

Clears previous speed values.

                                                                                               {
  xyze_pos_t machine = { rx, ry, rz, e };
  #if HAS_POSITION_MODIFIERS
  {
    apply_modifiers(machine
      #if HAS_LEVELING
        , true
      #endif
    );
  }
  #endif
  #if IS_KINEMATIC
    position_cart.set(rx, ry, rz, e);
    inverse_kinematics(machine);
    set_machine_position_mm(delta.a, delta.b, delta.c, machine.e);
  #else
    set_machine_position_mm(machine);
  #endif
}

Here is the call graph for this function:

Here is the caller graph for this function:

set_position_mm() [2/2]

static FORCE_INLINE void Planner::set_position_mm ( const xyze_pos_t &  cart )

static

{ set_position_mm(cart.x, cart.y, cart.z, cart.e); }

Here is the call graph for this function:

Here is the caller graph for this function:

set_e_position_mm()

void Planner::set_e_position_mm ( const float &  e )

static

Setters for planner position (also setting stepper position).

                                              {
  const uint8_t axis_index = E_AXIS_N(active_extruder);
  #if ENABLED(DISTINCT_E_FACTORS)
    last_extruder = active_extruder;
  #endif
  #if ENABLED(FWRETRACT)
    float e_new = e - fwretract.current_retract[active_extruder];
  #else
    const float e_new = e;
  #endif
  position.e = LROUND(settings.axis_steps_per_mm[axis_index] * e_new);
  #if HAS_POSITION_FLOAT
    position_float.e = e_new;
  #endif
  #if IS_KINEMATIC
    position_cart.e = e;
  #endif
  if (has_blocks_queued())
    buffer_sync_block();
  else
    stepper.set_axis_position(E_AXIS, position.e);
}

Here is the call graph for this function:

Here is the caller graph for this function:

set_machine_position_mm() [1/2]

void Planner::set_machine_position_mm ( const float &  a, const float &  b, const float &  c, const float &  e  )

static

Set the planner.position and individual stepper positions.

The supplied position is in machine space, and no additional conversions are applied.

Directly set the planner ABC position (and stepper positions) converting mm (or angles for SCARA) into steps.

The provided ABC position is in machine units.

                                                                                                    {
  #if ENABLED(DISTINCT_E_FACTORS)
    last_extruder = active_extruder;
  #endif
  #if HAS_POSITION_FLOAT
    position_float.set(a, b, c, e);
  #endif
  position.set(LROUND(a * settings.axis_steps_per_mm[A_AXIS]),
               LROUND(b * settings.axis_steps_per_mm[B_AXIS]),
               LROUND(c * settings.axis_steps_per_mm[C_AXIS]),
               LROUND(e * settings.axis_steps_per_mm[E_AXIS_N(active_extruder)]));
  if (has_blocks_queued()) {
    //previous_nominal_speed_sqr = 0.0; // Reset planner junction speeds. Assume start from rest.
    //previous_speed.reset();
    buffer_sync_block();
  }
  else
    stepper.set_position(position);
}

Here is the call graph for this function:

Here is the caller graph for this function:

set_machine_position_mm() [2/2]

static FORCE_INLINE void Planner::set_machine_position_mm ( const abce_pos_t &  abce )

static

{ set_machine_position_mm(abce.a, abce.b, abce.c, abce.e); }

Here is the call graph for this function:

Here is the caller graph for this function:

get_axis_position_mm()

float Planner::get_axis_position_mm ( const AxisEnum  axis )

static

Get an axis position according to stepper position(s) For CORE machines apply translation from ABC to XYZ.

                                                       {
  float axis_steps;
  #if IS_CORE
    // Requesting one of the "core" axes?
    if (axis == CORE_AXIS_1 || axis == CORE_AXIS_2) {
 
      // Protect the access to the position.
      const bool was_enabled = STEPPER_ISR_ENABLED();
      if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
 
      const int32_t p1 = stepper.position(CORE_AXIS_1),
                    p2 = stepper.position(CORE_AXIS_2);
 
      if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
 
      // ((a1+a2)+(a1-a2))/2 -> (a1+a2+a1-a2)/2 -> (a1+a1)/2 -> a1
      // ((a1+a2)-(a1-a2))/2 -> (a1+a2-a1+a2)/2 -> (a2+a2)/2 -> a2
      axis_steps = (axis == CORE_AXIS_2 ? CORESIGN(p1 - p2) : p1 + p2) * 0.5f;
    }
    else
      axis_steps = stepper.position(axis);
  #else
    axis_steps = stepper.position(axis);
  #endif
  return axis_steps * steps_to_mm[axis];
}

Here is the call graph for this function:

Here is the caller graph for this function:

quick_stop()

void Planner::quick_stop ( )

static

                         {
 
  // Remove all the queued blocks. Note that this function is NOT
  // called from the Stepper ISR, so we must consider tail as readonly!
  // that is why we set head to tail - But there is a race condition that
  // must be handled: The tail could change between the read and the assignment
  // so this must be enclosed in a critical section
 
  const bool was_enabled = STEPPER_ISR_ENABLED();
  if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
 
  // Drop all queue entries
  block_buffer_nonbusy = block_buffer_planned = block_buffer_head = block_buffer_tail;
 
  // Restart the block delay for the first movement - As the queue was
  // forced to empty, there's no risk the ISR will touch this.
  delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
 
  #if HAS_SPI_LCD
    // Clear the accumulated runtime
    clear_block_buffer_runtime();
  #endif
 
  // Make sure to drop any attempt of queuing moves for at least 1 second
  cleaning_buffer_counter = 1000;
 
  // Reenable Stepper ISR
  if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
 
  // And stop the stepper ISR
  stepper.quick_stop();
}

Here is the call graph for this function:

Here is the caller graph for this function:

endstop_triggered()

void Planner::endstop_triggered ( const AxisEnum  axis )

static

                                                   {
  // Record stepper position and discard the current block
  stepper.endstop_triggered(axis);
}

Here is the call graph for this function:

Here is the caller graph for this function:

triggered_position_mm()

float Planner::triggered_position_mm ( const AxisEnum  axis )

static

                                                        {
  return stepper.triggered_position(axis) * steps_to_mm[axis];
}

Here is the call graph for this function:

synchronize()

void Planner::synchronize ( )

static

Block until all buffered steps are executed / cleaned

                          {
  while (
    has_blocks_queued() || cleaning_buffer_counter
    #if ENABLED(EXTERNAL_CLOSED_LOOP_CONTROLLER)
      || (READ(CLOSED_LOOP_ENABLE_PIN) && !READ(CLOSED_LOOP_MOVE_COMPLETE_PIN))
    #endif
  ) idle();
}

Here is the call graph for this function:

Here is the caller graph for this function:

finish_and_disable()

void Planner::finish_and_disable ( )

static

                                 {
  while (has_blocks_queued() || cleaning_buffer_counter) idle();
  disable_all_steppers();
}

Here is the call graph for this function:

tick()

static void Planner::tick ( )

static

                       {
      if (cleaning_buffer_counter) {
        --cleaning_buffer_counter;
        #if ENABLED(SD_FINISHED_STEPPERRELEASE) && defined(SD_FINISHED_RELEASECOMMAND)
          if (!cleaning_buffer_counter) queue.inject_P(PSTR(SD_FINISHED_RELEASECOMMAND));
        #endif
      }
    }

Here is the call graph for this function:

Here is the caller graph for this function:

has_blocks_queued()

static FORCE_INLINE bool Planner::has_blocks_queued ( )

static

Does the buffer have any blocks queued?

{ return (block_buffer_head != block_buffer_tail); }

Here is the caller graph for this function:

get_current_block()

static block_t* Planner::get_current_block ( )

static

The current block. nullptr if the buffer is empty. This also marks the block as busy. WARNING: Called from Stepper ISR context!

                                        {
 
      // Get the number of moves in the planner queue so far
      const uint8_t nr_moves = movesplanned();
 
      // If there are any moves queued ...
      if (nr_moves) {
 
        // If there is still delay of delivery of blocks running, decrement it
        if (delay_before_delivering) {
          --delay_before_delivering;
          // If the number of movements queued is less than 3, and there is still time
          //  to wait, do not deliver anything
          if (nr_moves < 3 && delay_before_delivering) return nullptr;
          delay_before_delivering = 0;
        }
 
        // If we are here, there is no excuse to deliver the block
        block_t * const block = &block_buffer[block_buffer_tail];
 
        // No trapezoid calculated? Don't execute yet.
        if (TEST(block->flag, BLOCK_BIT_RECALCULATE)) return nullptr;
 
        #if HAS_SPI_LCD
          block_buffer_runtime_us -= block->segment_time_us; // We can't be sure how long an active block will take, so don't count it.
        #endif
 
        // As this block is busy, advance the nonbusy block pointer
        block_buffer_nonbusy = next_block_index(block_buffer_tail);
 
        // Push block_buffer_planned pointer, if encountered.
        if (block_buffer_tail == block_buffer_planned)
          block_buffer_planned = block_buffer_nonbusy;
 
        // Return the block
        return block;
      }
 
      // The queue became empty
      #if HAS_SPI_LCD
        clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero.
      #endif
 
      return nullptr;
    }

Here is the call graph for this function:

Here is the caller graph for this function:

discard_current_block()

static FORCE_INLINE void Planner::discard_current_block ( )

static

"Discard" the block and "release" the memory. Called when the current block is no longer needed. NB: There MUST be a current block to call this function!!

                                                     {
      if (has_blocks_queued())
        block_buffer_tail = next_block_index(block_buffer_tail);
    }

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

block_buffer

block_t Planner::block_buffer

static

The move buffer, calculated in stepper steps

block_buffer is a ring buffer...

        head,tail : indexes for write,read
       head==tail : the buffer is empty
       head!=tail : blocks are in the buffer

head==(tail-1)size : the buffer is full

Writer of head is Planner::buffer_segment(). Reader of tail is Stepper::isr(). Always consider tail busy / read-only

A ring buffer of moves described in steps

block_buffer_head

volatile uint8_t Planner::block_buffer_head

static

block_buffer_nonbusy

volatile uint8_t Planner::block_buffer_nonbusy

static

block_buffer_planned

volatile uint8_t Planner::block_buffer_planned

static

block_buffer_tail

volatile uint8_t Planner::block_buffer_tail

static

cleaning_buffer_counter

uint16_t Planner::cleaning_buffer_counter

static

delay_before_delivering

uint8_t Planner::delay_before_delivering

static

settings

planner_settings_t Planner::settings

static

max_acceleration_steps_per_s2

uint32_t Planner::max_acceleration_steps_per_s2

static

steps_to_mm

float Planner::steps_to_mm

static

leveling_active

bool Planner::leveling_active = false

static

bed_level_matrix

matrix_3x3 Planner::bed_level_matrix

static

position_float

xyze_pos_t Planner::position_float

static

skew_factor

skew_factor_t Planner::skew_factor

static