From 9c29c634b7d61e55af109de6cafd0c3899ebafe4 Mon Sep 17 00:00:00 2001 From: Harald Wagener Date: Thu, 11 Dec 2025 01:09:05 +0100 Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20NO=5FSTANDARD=5FMOTION=20(#28212)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Scott Lahteine --- Marlin/Configuration_adv.h | 6 +- Marlin/src/HAL/ESP32/i2s.cpp | 51 +- Marlin/src/gcode/feature/ft_motion/M493.cpp | 20 +- Marlin/src/inc/Conditionals-4-adv.h | 64 +- Marlin/src/inc/SanityCheck.h | 19 + Marlin/src/lcd/menu/menu_motion.cpp | 6 +- Marlin/src/module/ft_motion.cpp | 6 +- Marlin/src/module/ft_motion.h | 80 +- Marlin/src/module/ft_motion/shaping.h | 1 + Marlin/src/module/planner.cpp | 297 +-- Marlin/src/module/planner.h | 17 +- Marlin/src/module/stepper.cpp | 2084 ++++++++++--------- Marlin/src/module/stepper.h | 41 +- buildroot/tests/rambo | 3 +- 14 files changed, 1398 insertions(+), 1297 deletions(-) diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 1f56d184fc..f217923520 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1160,7 +1160,11 @@ #if ENABLED(FT_MOTION) //#define FTM_IS_DEFAULT_MOTION // Use FT Motion as the factory default? //#define FT_MOTION_MENU // Provide a MarlinUI menu to set M493 and M494 parameters - //#define FTM_HOME_AND_PROBE // Use FT Motion for homing / probing. Disable if FT Motion breaks these functions. + + //#define NO_STANDARD_MOTION // Disable the standard motion system entirely to save Flash and RAM + #if DISABLED(NO_STANDARD_MOTION) + //#define FTM_HOME_AND_PROBE // Use FT Motion for homing / probing. Disable if FT Motion breaks these functions. + #endif //#define FTM_DYNAMIC_FREQ // Enable for linear adjustment of XY shaping frequency according to Z or E #if ENABLED(FTM_DYNAMIC_FREQ) diff --git a/Marlin/src/HAL/ESP32/i2s.cpp b/Marlin/src/HAL/ESP32/i2s.cpp index 6aeeb0e3dc..f7c01730b3 100644 --- a/Marlin/src/HAL/ESP32/i2s.cpp +++ b/Marlin/src/HAL/ESP32/i2s.cpp @@ -156,38 +156,43 @@ void stepperTask(void *parameter) { while (dma.rw_pos < DMA_SAMPLE_COUNT) { - #if ENABLED(FT_MOTION) + if (using_ftMotion) { - if (using_ftMotion) { + #if ENABLED(FT_MOTION) if (!nextMainISR) stepper.ftMotion_stepper(); nextMainISR = 0; - } + #endif - #endif + } + else { - if (!using_ftMotion) { - if (!nextMainISR) { - stepper.pulse_phase_isr(); - nextMainISR = stepper.block_phase_isr(); - } - #if ENABLED(LIN_ADVANCE) - else if (!nextAdvanceISR) { - stepper.advance_isr(); - nextAdvanceISR = stepper.la_interval; + #if HAS_STANDARD_MOTION + + if (!nextMainISR) { + stepper.pulse_phase_isr(); + nextMainISR = stepper.block_phase_isr(); } - #endif - else - i2s_push_sample(); + #if ENABLED(LIN_ADVANCE) + else if (!nextAdvanceISR) { + stepper.advance_isr(); + nextAdvanceISR = stepper.la_interval; + } + #endif + else + i2s_push_sample(); - nextMainISR--; + nextMainISR--; - #if ENABLED(LIN_ADVANCE) - if (nextAdvanceISR == stepper.LA_ADV_NEVER) - nextAdvanceISR = stepper.la_interval; + #if ENABLED(LIN_ADVANCE) + if (nextAdvanceISR == stepper.LA_ADV_NEVER) + nextAdvanceISR = stepper.la_interval; + + if (nextAdvanceISR && nextAdvanceISR != stepper.LA_ADV_NEVER) + nextAdvanceISR--; + #endif + + #endif // HAS_STANDARD_MOTION - if (nextAdvanceISR && nextAdvanceISR != stepper.LA_ADV_NEVER) - nextAdvanceISR--; - #endif } } } diff --git a/Marlin/src/gcode/feature/ft_motion/M493.cpp b/Marlin/src/gcode/feature/ft_motion/M493.cpp index e3f4bea8cb..528102c868 100644 --- a/Marlin/src/gcode/feature/ft_motion/M493.cpp +++ b/Marlin/src/gcode/feature/ft_motion/M493.cpp @@ -151,7 +151,7 @@ void GcodeSuite::M493_report(const bool forReplay/*=true*/) { /** * M493: Set Fixed-time Motion Control parameters * - * S Set Fixed-Time motion mode on or off. + * S Set Fixed-Time motion mode on or off. Ignored with NO_STANDARD_MOTION. * 0: Fixed-Time Motion OFF (Standard Motion) * 1: Fixed-Time Motion ON * @@ -210,15 +210,17 @@ void GcodeSuite::M493() { ft_config_t &c = ftMotion.cfg; - // Parse 'S' mode parameter. - if (parser.seen('S')) { - const bool active = parser.value_bool(); - if (active != c.active) { - stepper.ftMotion_syncPosition(); - c.active = active; - flag.report = true; + #if HAS_STANDARD_MOTION + // Parse 'S' mode parameter. + if (parser.seen('S')) { + const bool active = parser.value_bool(); + if (active != c.active) { + stepper.ftMotion_syncPosition(); + c.active = active; + flag.report = true; + } } - } + #endif #if NUM_AXES_SHAPED > 0 diff --git a/Marlin/src/inc/Conditionals-4-adv.h b/Marlin/src/inc/Conditionals-4-adv.h index 41e2e914f1..f92ba2c079 100644 --- a/Marlin/src/inc/Conditionals-4-adv.h +++ b/Marlin/src/inc/Conditionals-4-adv.h @@ -346,10 +346,46 @@ #define HAS_CLASSIC_E_JERK 1 #endif -// Linear advance uses Jerk since E is an isolated axis -#if ALL(FT_MOTION, HAS_EXTRUDERS) - #define FTM_HAS_LIN_ADVANCE 1 +// Fixed-Time Motion +#if ENABLED(FT_MOTION) + #if HAS_X_AXIS + #define HAS_FTM_SHAPING 1 + #define FTM_SHAPER_X + #endif + #if HAS_Y_AXIS + #define FTM_SHAPER_Y + #endif + #if !HAS_Z_AXIS + #undef FTM_SHAPER_Z + #endif + #if HAS_EXTRUDERS + #define FTM_HAS_LIN_ADVANCE 1 + #else + #undef FTM_SHAPER_E + #endif + #if ENABLED(NO_STANDARD_MOTION) + #define FTM_HOME_AND_PROBE + #undef LIN_ADVANCE + #undef SMOOTH_LIN_ADVANCE + #undef S_CURVE_ACCELERATION + #undef ADAPTIVE_STEP_SMOOTHING + #undef INPUT_SHAPING_X + #undef INPUT_SHAPING_Y + #undef INPUT_SHAPING_E_SYNC + #undef MULTISTEPPING_LIMIT + #define MULTISTEPPING_LIMIT 1 + #endif #endif +#if DISABLED(NO_STANDARD_MOTION) + #define HAS_STANDARD_MOTION 1 +#endif + +// ZV Input shaping +#if ANY(INPUT_SHAPING_X, INPUT_SHAPING_Y, INPUT_SHAPING_Z) + #define HAS_ZV_SHAPING 1 +#endif + +// Linear advance uses Jerk since E is an isolated axis #if ANY(FTM_HAS_LIN_ADVANCE, LIN_ADVANCE) #define HAS_LIN_ADVANCE_K 1 #endif @@ -1521,28 +1557,6 @@ #define CANNOT_EMBED_CONFIGURATION defined(__AVR__) #endif -// Input shaping -#if ANY(INPUT_SHAPING_X, INPUT_SHAPING_Y, INPUT_SHAPING_Z) - #define HAS_ZV_SHAPING 1 -#endif - -// FT Motion: Shapers -#if ENABLED(FT_MOTION) - #if HAS_X_AXIS - #define HAS_FTM_SHAPING 1 - #define FTM_SHAPER_X - #endif - #if HAS_Y_AXIS - #define FTM_SHAPER_Y - #endif - #if !HAS_Z_AXIS - #undef FTM_SHAPER_Z - #endif - #if !HAS_EXTRUDERS - #undef FTM_SHAPER_E - #endif -#endif - // Multi-Stepping Limit #ifndef MULTISTEPPING_LIMIT #define MULTISTEPPING_LIMIT 128 diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index 868b0e9fb8..88d177cde3 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -4500,6 +4500,25 @@ static_assert(_PLUS_TEST(3), "DEFAULT_MAX_ACCELERATION values must be positive." #if ENABLED(FTM_RESONANCE_TEST) && DISABLED(EMERGENCY_PARSER) #error "EMERGENCY_PARSER is required with FTM_RESONANCE_TEST (to cancel the test)." #endif + #if !HAS_STANDARD_MOTION + #if ENABLED(NONLINEAR_EXTRUSION) + #error "NONLINEAR_EXTRUSION is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it." + #elif ENABLED(SMOOTH_LIN_ADVANCE) + #error "SMOOTH_LIN_ADVANCE is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it." + #elif ENABLED(MIXING_EXTRUDER) + #error "MIXING_EXTRUDER is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it." + #elif ENABLED(FREEZE_FEATURE) + #error "FREEZE_FEATURE is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it." + #elif ENABLED(DIRECT_STEPPING) + #error "DIRECT_STEPPING is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it." + #elif ENABLED(DIFFERENTIAL_EXTRUDER) + #error "DIFFERENTIAL_EXTRUDER is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it." + #elif ENABLED(LASER_FEATURE) + #error "LASER_FEATURE is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it." + #elif ENABLED(Z_LATE_ENABLE) + #error "Z_LATE_ENABLE is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it." + #endif + #endif #endif // Multi-Stepping Limit diff --git a/Marlin/src/lcd/menu/menu_motion.cpp b/Marlin/src/lcd/menu/menu_motion.cpp index 8f644361f4..bbd6b5beff 100644 --- a/Marlin/src/lcd/menu/menu_motion.cpp +++ b/Marlin/src/lcd/menu/menu_motion.cpp @@ -499,8 +499,10 @@ void menu_move() { START_MENU(); BACK_ITEM(MSG_MOTION); - bool show_state = c.active; - EDIT_ITEM(bool, MSG_FIXED_TIME_MOTION, &show_state, []{ (void)ftMotion.toggle(); }); + #if HAS_STANDARD_MOTION + bool show_state = c.active; + EDIT_ITEM(bool, MSG_FIXED_TIME_MOTION, &show_state, []{ (void)ftMotion.toggle(); }); + #endif // Show only when FT Motion is active (or optionally always show) if (c.active || ENABLED(FT_MOTION_NO_MENU_TOGGLE)) { diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index a2e0b8be70..d8cc2e5f45 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -375,12 +375,8 @@ bool FTMotion::plan_next_block() { const xyze_pos_t& moveDist = current_block->dist_mm; ratio = moveDist / totalLength; - const float mmps = totalLength / current_block->step_event_count, // (mm/step) Distance for each step - initial_speed = mmps * current_block->initial_rate, // (mm/s) Start feedrate - final_speed = mmps * current_block->final_rate; // (mm/s) End feedrate - // Plan the trajectory using the trajectory generator - currentGenerator->plan(initial_speed, final_speed, current_block->acceleration, current_block->nominal_speed, totalLength); + currentGenerator->plan(current_block->entry_speed, current_block->exit_speed, current_block->acceleration, current_block->nominal_speed, totalLength); endPos_prevBlock += moveDist; diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h index a149057547..991d3efcef 100644 --- a/Marlin/src/module/ft_motion.h +++ b/Marlin/src/module/ft_motion.h @@ -58,7 +58,11 @@ * FTConfig - The active configured state of FT Motion */ typedef struct FTConfig { - bool active = ENABLED(FTM_IS_DEFAULT_MOTION); // Active (else standard motion) + #if HAS_STANDARD_MOTION + bool active = ENABLED(FTM_IS_DEFAULT_MOTION); // Active (else Standard Motion) + #else + static constexpr bool active = true; // Always active with NO_STANDARD_MOTION + #endif bool axis_sync_enabled = true; // Axis synchronization enabled #if HAS_FTM_SHAPING @@ -104,6 +108,33 @@ typedef struct FTConfig { constexpr bool goodBaseFreq(const float f) { return WITHIN(f, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2); } + void set_defaults() { + #if HAS_STANDARD_MOTION + active = ENABLED(FTM_IS_DEFAULT_MOTION); + #endif + + #if HAS_FTM_SHAPING + + #define _SET_CFG_DEFAULTS(A) do{ \ + shaper.A = FTM_DEFAULT_SHAPER_##A; \ + baseFreq.A = FTM_SHAPING_DEFAULT_FREQ_##A; \ + zeta.A = FTM_SHAPING_ZETA_##A; \ + vtol.A = FTM_SHAPING_V_TOL_##A; \ + }while(0); + + SHAPED_MAP(_SET_CFG_DEFAULTS); + #undef _SET_CFG_DEFAULTS + + #if HAS_DYNAMIC_FREQ + dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE; + dynFreqK.reset(); + #endif + + #endif // HAS_FTM_SHAPING + + TERN_(FTM_POLYS, poly6_acceleration_overshoot = FTM_POLY6_ACCELERATION_OVERSHOOT); + } + } ft_config_t; /** @@ -122,31 +153,9 @@ class FTMotion { static bool busy; static void set_defaults() { - cfg.active = ENABLED(FTM_IS_DEFAULT_MOTION); + cfg.set_defaults(); - #if HAS_FTM_SHAPING - - #define _SET_CFG_DEFAULTS(A) do{ \ - cfg.shaper.A = FTM_DEFAULT_SHAPER_##A; \ - cfg.baseFreq.A = FTM_SHAPING_DEFAULT_FREQ_##A; \ - cfg.zeta.A = FTM_SHAPING_ZETA_##A; \ - cfg.vtol.A = FTM_SHAPING_V_TOL_##A; \ - }while(0); - - SHAPED_MAP(_SET_CFG_DEFAULTS); - #undef _SET_CFG_DEFAULTS - - #if HAS_DYNAMIC_FREQ - cfg.dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE; - //ZERO(cfg.dynFreqK); - #define _DYN_RESET(A) cfg.dynFreqK.A = 0.0f; - SHAPED_MAP(_DYN_RESET); - #undef _DYN_RESET - #endif - - update_shaping_params(); - - #endif // HAS_FTM_SHAPING + TERN_(HAS_FTM_SHAPING, update_shaping_params()); #if ENABLED(FTM_SMOOTHING) #define _SET_SMOOTH(A) set_smoothing_time(_AXIS(A), FTM_SMOOTHING_TIME_##A); @@ -154,10 +163,7 @@ class FTMotion { #undef _SET_SMOOTH #endif - #if ENABLED(FTM_POLYS) - cfg.poly6_acceleration_overshoot = FTM_POLY6_ACCELERATION_OVERSHOOT; - setTrajectoryType(TrajectoryType::FTM_TRAJECTORY_TYPE); - #endif + TERN_(FTM_POLYS, setTrajectoryType(TrajectoryType::FTM_TRAJECTORY_TYPE)); reset(); } @@ -188,12 +194,14 @@ class FTMotion { static void reset(); // Reset all states of the fixed time conversion to defaults. // Safely toggle the active state of FT Motion - static bool toggle() { - stepper.ftMotion_syncPosition(); - FLIP(cfg.active); - update_shaping_params(); - return cfg.active; - } + #if ALL(FT_MOTION, HAS_STANDARD_MOTION) + static bool toggle() { + stepper.ftMotion_syncPosition(); + FLIP(cfg.active); + update_shaping_params(); + return cfg.active; + } + #endif // Trajectory generator selection static void setTrajectoryType(const TrajectoryType type); @@ -201,7 +209,7 @@ class FTMotion { static FSTR_P getTrajectoryName(); FORCE_INLINE static bool axis_is_moving(const AxisEnum axis) { - return cfg.active ? moving_axis_flags[axis] : stepper.axis_is_moving(axis); + return cfg.active ? moving_axis_flags[axis] : TERN0(HAS_STANDARD_MOTION, stepper.axis_is_moving(axis)); } FORCE_INLINE static bool motor_direction(const AxisEnum axis) { return cfg.active ? axis_move_dir[axis] : stepper.last_direction_bits[axis]; diff --git a/Marlin/src/module/ft_motion/shaping.h b/Marlin/src/module/ft_motion/shaping.h index 5b37c2930d..dacd0454fb 100644 --- a/Marlin/src/module/ft_motion/shaping.h +++ b/Marlin/src/module/ft_motion/shaping.h @@ -73,6 +73,7 @@ struct FTShapedAxes { T& operator[](const int axis) { return val[axis_to_index(axis)]; } + void reset() { ZERO(val); } private: static constexpr int axis_to_index(const int axis) { diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 62db79a907..f39a358457 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -797,141 +797,150 @@ block_t* Planner::get_future_block(const uint8_t offset) { */ void Planner::calculate_trapezoid_for_block(block_t * const block, const float entry_speed, const float exit_speed) { - const float spmm = block->steps_per_mm; - uint32_t initial_rate = entry_speed ? LROUND(entry_speed * spmm) : block->initial_rate, - final_rate = LROUND(exit_speed * spmm); - - NOLESS(initial_rate, stepper.minimal_step_rate); - NOLESS(final_rate, stepper.minimal_step_rate); - NOLESS(block->nominal_rate, stepper.minimal_step_rate); - - #if ANY(S_CURVE_ACCELERATION, LIN_ADVANCE) - // If we have some plateau time, the cruise rate will be the nominal rate - uint32_t cruise_rate = block->nominal_rate; + #if ENABLED(FT_MOTION) + block->entry_speed = entry_speed; + block->exit_speed = exit_speed; #endif - // Steps for acceleration, plateau and deceleration - int32_t plateau_steps = block->step_event_count, - accelerate_steps = 0, - decelerate_steps = 0; + #if HAS_STANDARD_MOTION - const int32_t accel = block->acceleration_steps_per_s2; - float inverse_accel = 0.0f; - if (accel != 0) { - inverse_accel = 1.0f / accel; - const float half_inverse_accel = 0.5f * inverse_accel, - nominal_rate_sq = FLOAT_SQ(block->nominal_rate), - // Steps required for acceleration, deceleration to/from nominal rate - decelerate_steps_float = half_inverse_accel * (nominal_rate_sq - FLOAT_SQ(final_rate)), - accelerate_steps_float = half_inverse_accel * (nominal_rate_sq - FLOAT_SQ(initial_rate)); - // Aims to fully reach nominal and final rates - accelerate_steps = CEIL(accelerate_steps_float); - decelerate_steps = CEIL(decelerate_steps_float); + const float spmm = block->steps_per_mm; + uint32_t initial_rate = entry_speed ? LROUND(entry_speed * spmm) : block->initial_rate, + final_rate = LROUND(exit_speed * spmm); - // Steps between acceleration and deceleration, if any - plateau_steps -= accelerate_steps + decelerate_steps; + NOLESS(initial_rate, stepper.minimal_step_rate); + NOLESS(final_rate, stepper.minimal_step_rate); + NOLESS(block->nominal_rate, stepper.minimal_step_rate); - // Does accelerate_steps + decelerate_steps exceed step_event_count? - // Then we can't possibly reach the nominal rate, there will be no cruising. - // Calculate accel / braking time in order to reach the final_rate exactly - // at the end of this block. - if (plateau_steps < 0) { - accelerate_steps = LROUND((block->step_event_count + accelerate_steps_float - decelerate_steps_float) * 0.5f); - LIMIT(accelerate_steps, 0, int32_t(block->step_event_count)); - decelerate_steps = block->step_event_count - accelerate_steps; + #if ANY(S_CURVE_ACCELERATION, LIN_ADVANCE) + // If we have some plateau time, the cruise rate will be the nominal rate + uint32_t cruise_rate = block->nominal_rate; + #endif - #if ANY(S_CURVE_ACCELERATION, LIN_ADVANCE) - // We won't reach the cruising rate. Let's calculate the speed we will reach - NOMORE(cruise_rate, final_speed(initial_rate, accel, accelerate_steps)); - #endif - } - } + // Steps for acceleration, plateau and deceleration + int32_t plateau_steps = block->step_event_count, + accelerate_steps = 0, + decelerate_steps = 0; - #if ANY(S_CURVE_ACCELERATION, SMOOTH_LIN_ADVANCE) - const float rate_factor = inverse_accel * (STEPPER_TIMER_RATE); - // Jerk controlled speed requires to express speed versus time, NOT steps - uint32_t acceleration_time = rate_factor * float(cruise_rate - initial_rate), - deceleration_time = rate_factor * float(cruise_rate - final_rate); - #endif - #if ENABLED(S_CURVE_ACCELERATION) - // And to offload calculations from the ISR, we also calculate the inverse of those times here - uint32_t acceleration_time_inverse = get_period_inverse(acceleration_time), - deceleration_time_inverse = get_period_inverse(deceleration_time); - #endif + const int32_t accel = block->acceleration_steps_per_s2; + float inverse_accel = 0.0f; + if (accel != 0) { + inverse_accel = 1.0f / accel; + const float half_inverse_accel = 0.5f * inverse_accel, + nominal_rate_sq = FLOAT_SQ(block->nominal_rate), + // Steps required for acceleration, deceleration to/from nominal rate + decelerate_steps_float = half_inverse_accel * (nominal_rate_sq - FLOAT_SQ(final_rate)), + accelerate_steps_float = half_inverse_accel * (nominal_rate_sq - FLOAT_SQ(initial_rate)); + // Aims to fully reach nominal and final rates + accelerate_steps = CEIL(accelerate_steps_float); + decelerate_steps = CEIL(decelerate_steps_float); - // Store new block parameters - block->accelerate_before = accelerate_steps; - block->decelerate_start = block->step_event_count - decelerate_steps; - block->initial_rate = initial_rate; - block->final_rate = final_rate; + // Steps between acceleration and deceleration, if any + plateau_steps -= accelerate_steps + decelerate_steps; - #if ANY(S_CURVE_ACCELERATION, SMOOTH_LIN_ADVANCE) - block->acceleration_time = acceleration_time; - block->deceleration_time = deceleration_time; - block->cruise_rate = cruise_rate; - #endif - #if ENABLED(S_CURVE_ACCELERATION) - block->acceleration_time_inverse = acceleration_time_inverse; - block->deceleration_time_inverse = deceleration_time_inverse; - #endif + // Does accelerate_steps + decelerate_steps exceed step_event_count? + // Then we can't possibly reach the nominal rate, there will be no cruising. + // Calculate accel / braking time in order to reach the final_rate exactly + // at the end of this block. + if (plateau_steps < 0) { + accelerate_steps = LROUND((block->step_event_count + accelerate_steps_float - decelerate_steps_float) * 0.5f); + LIMIT(accelerate_steps, 0, int32_t(block->step_event_count)); + decelerate_steps = block->step_event_count - accelerate_steps; - #if ENABLED(SMOOTH_LIN_ADVANCE) - block->cruise_time = plateau_steps > 0 ? float(plateau_steps) * float(STEPPER_TIMER_RATE) / float(cruise_rate) : 0; - #elif HAS_ROUGH_LIN_ADVANCE - if (block->la_advance_rate) { - const float comp = get_advance_k(block->extruder) * block->steps.e / block->step_event_count; - block->max_adv_steps = cruise_rate * comp; - block->final_adv_steps = final_rate * comp; - } - #endif - - #if ENABLED(LASER_POWER_TRAP) - /** - * Laser Trapezoid Calculations - * - * Approximate the trapezoid with the laser, incrementing the power every `trap_ramp_entry_incr` - * steps while accelerating, and decrementing the power every `trap_ramp_exit_decr` while decelerating, - * to keep power proportional to feedrate. Laser power trap will reduce the initial power to no less - * than the laser_power_floor value. Based on the number of calculated accel/decel steps the power is - * distributed over the trapezoid entry- and exit-ramp steps. - * - * trap_ramp_active_pwr - The active power is initially set at a reduced level factor of initial - * power / accel steps and will be additively incremented using a trap_ramp_entry_incr value for each - * accel step processed later in the stepper code. The trap_ramp_exit_decr value is calculated as - * power / decel steps and is also adjusted to no less than the power floor. - * - * If the power == 0 the inline mode variables need to be set to zero to prevent stepper processing. - * The method allows for simpler non-powered moves like G0 or G28. - * - * Laser Trap Power works for all Jerk and Curve modes; however Arc-based moves will have issues since - * the segments are usually too small. - */ - if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS - && planner.laser_inline.status.isPowered && planner.laser_inline.status.isEnabled - ) { - if (block->laser.power > 0) { - NOLESS(block->laser.power, laser_power_floor); - block->laser.trap_ramp_active_pwr = (block->laser.power - laser_power_floor) * (initial_rate / float(block->nominal_rate)) + laser_power_floor; - block->laser.trap_ramp_entry_incr = (block->laser.power - block->laser.trap_ramp_active_pwr) / accelerate_steps; - float laser_pwr = block->laser.power * (final_rate / float(block->nominal_rate)); - NOLESS(laser_pwr, laser_power_floor); - block->laser.trap_ramp_exit_decr = (block->laser.power - laser_pwr) / decelerate_steps; - #if ENABLED(DEBUG_LASER_TRAP) - SERIAL_ECHO_MSG("lp:", block->laser.power); - SERIAL_ECHO_MSG("as:", accelerate_steps); - SERIAL_ECHO_MSG("ds:", decelerate_steps); - SERIAL_ECHO_MSG("p.trap:", block->laser.trap_ramp_active_pwr); - SERIAL_ECHO_MSG("p.incr:", block->laser.trap_ramp_entry_incr); - SERIAL_ECHO_MSG("p.decr:", block->laser.trap_ramp_exit_decr); + #if ANY(S_CURVE_ACCELERATION, LIN_ADVANCE) + // We won't reach the cruising rate. Let's calculate the speed we will reach + NOMORE(cruise_rate, final_speed(initial_rate, accel, accelerate_steps)); #endif } - else { - block->laser.trap_ramp_active_pwr = 0; - block->laser.trap_ramp_entry_incr = 0; - block->laser.trap_ramp_exit_decr = 0; - } } - #endif // LASER_POWER_TRAP + + #if ANY(S_CURVE_ACCELERATION, SMOOTH_LIN_ADVANCE) + const float rate_factor = inverse_accel * (STEPPER_TIMER_RATE); + // Jerk controlled speed requires to express speed versus time, NOT steps + uint32_t acceleration_time = rate_factor * float(cruise_rate - initial_rate), + deceleration_time = rate_factor * float(cruise_rate - final_rate); + #endif + #if ENABLED(S_CURVE_ACCELERATION) + // And to offload calculations from the ISR, we also calculate the inverse of those times here + uint32_t acceleration_time_inverse = get_period_inverse(acceleration_time), + deceleration_time_inverse = get_period_inverse(deceleration_time); + #endif + + // Store new block parameters + block->accelerate_before = accelerate_steps; + block->decelerate_start = block->step_event_count - decelerate_steps; + block->initial_rate = initial_rate; + block->final_rate = final_rate; + + #if ANY(S_CURVE_ACCELERATION, SMOOTH_LIN_ADVANCE) + block->acceleration_time = acceleration_time; + block->deceleration_time = deceleration_time; + block->cruise_rate = cruise_rate; + #endif + #if ENABLED(S_CURVE_ACCELERATION) + block->acceleration_time_inverse = acceleration_time_inverse; + block->deceleration_time_inverse = deceleration_time_inverse; + #endif + + #if ENABLED(SMOOTH_LIN_ADVANCE) + block->cruise_time = plateau_steps > 0 ? float(plateau_steps) * float(STEPPER_TIMER_RATE) / float(cruise_rate) : 0; + #elif HAS_ROUGH_LIN_ADVANCE + if (block->la_advance_rate) { + const float comp = get_advance_k(block->extruder) * block->steps.e / block->step_event_count; + block->max_adv_steps = cruise_rate * comp; + block->final_adv_steps = final_rate * comp; + } + #endif + + #if ENABLED(LASER_POWER_TRAP) + /** + * Laser Trapezoid Calculations + * + * Approximate the trapezoid with the laser, incrementing the power every `trap_ramp_entry_incr` + * steps while accelerating, and decrementing the power every `trap_ramp_exit_decr` while decelerating, + * to keep power proportional to feedrate. Laser power trap will reduce the initial power to no less + * than the laser_power_floor value. Based on the number of calculated accel/decel steps the power is + * distributed over the trapezoid entry- and exit-ramp steps. + * + * trap_ramp_active_pwr - The active power is initially set at a reduced level factor of initial + * power / accel steps and will be additively incremented using a trap_ramp_entry_incr value for each + * accel step processed later in the stepper code. The trap_ramp_exit_decr value is calculated as + * power / decel steps and is also adjusted to no less than the power floor. + * + * If the power == 0 the inline mode variables need to be set to zero to prevent stepper processing. + * The method allows for simpler non-powered moves like G0 or G28. + * + * Laser Trap Power works for all Jerk and Curve modes; however Arc-based moves will have issues since + * the segments are usually too small. + */ + if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS + && planner.laser_inline.status.isPowered && planner.laser_inline.status.isEnabled + ) { + if (block->laser.power > 0) { + NOLESS(block->laser.power, laser_power_floor); + block->laser.trap_ramp_active_pwr = (block->laser.power - laser_power_floor) * (initial_rate / float(block->nominal_rate)) + laser_power_floor; + block->laser.trap_ramp_entry_incr = (block->laser.power - block->laser.trap_ramp_active_pwr) / accelerate_steps; + float laser_pwr = block->laser.power * (final_rate / float(block->nominal_rate)); + NOLESS(laser_pwr, laser_power_floor); + block->laser.trap_ramp_exit_decr = (block->laser.power - laser_pwr) / decelerate_steps; + #if ENABLED(DEBUG_LASER_TRAP) + SERIAL_ECHO_MSG("lp:", block->laser.power); + SERIAL_ECHO_MSG("as:", accelerate_steps); + SERIAL_ECHO_MSG("ds:", decelerate_steps); + SERIAL_ECHO_MSG("p.trap:", block->laser.trap_ramp_active_pwr); + SERIAL_ECHO_MSG("p.incr:", block->laser.trap_ramp_entry_incr); + SERIAL_ECHO_MSG("p.decr:", block->laser.trap_ramp_exit_decr); + #endif + } + else { + block->laser.trap_ramp_active_pwr = 0; + block->laser.trap_ramp_entry_incr = 0; + block->laser.trap_ramp_exit_decr = 0; + } + } + #endif // LASER_POWER_TRAP + + #endif // HAS_STANDARD_MOTION } /** @@ -1131,12 +1140,18 @@ void Planner::recalculate_trapezoids(const float safe_exit_speed_sqr) { if (stepper.is_block_busy(block)) { // Block is BUSY so we can't change the exit speed. Revert any reverse pass change. next->entry_speed_sqr = next->min_entry_speed_sqr; - if (!next->initial_rate) { - // 'next' was never calculated. Planner is falling behind so for maximum efficiency - // set next's stepping speed directly and forgo checking against min_entry_speed_sqr. - // calculate_trapezoid_for_block() can handle it, albeit sub-optimally. - next->initial_rate = block->final_rate; - } + + // If 'next' was never calculated the Planner is falling behind, so for maximum efficiency + // set next's stepping speed directly and forego checking against min_entry_speed_sqr. + // calculate_trapezoid_for_block() can handle it, albeit sub-optimally. + + #if HAS_STANDARD_MOTION + if (!next->initial_rate) next->initial_rate = block->final_rate; + #endif + #if ENABLED(FT_MOTION) + if (!next->entry_speed) next->entry_speed = block->exit_speed; + #endif + // Note that at this point next_entry_speed is (still) 0. } else { @@ -2108,7 +2123,7 @@ bool Planner::_populate_block( NUM_AXIS_CODE( if (block->steps.x) stepper.enable_axis(X_AXIS), if (block->steps.y) stepper.enable_axis(Y_AXIS), - if (TERN(Z_LATE_ENABLE, 0, block->steps.z)) stepper.enable_axis(Z_AXIS), + if (TERN(Z_LATE_ENABLE, false, block->steps.z)) stepper.enable_axis(Z_AXIS), if (block->steps.i) stepper.enable_axis(I_AXIS), if (block->steps.j) stepper.enable_axis(J_AXIS), if (block->steps.k) stepper.enable_axis(K_AXIS), @@ -2224,8 +2239,10 @@ bool Planner::_populate_block( if (was_enabled) stepper.wake_up(); #endif - block->nominal_speed = block->millimeters * inverse_secs; // (mm/sec) Always > 0 - block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0 + block->nominal_speed = block->millimeters * inverse_secs; // (mm/sec) Always > 0 + #if HAS_STANDARD_MOTION + block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0 + #endif #if ENABLED(FILAMENT_WIDTH_SENSOR) if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM) // Only for extruder with filament sensor @@ -2317,7 +2334,7 @@ bool Planner::_populate_block( // Correct the speed if (speed_factor < 1.0f) { current_speed *= speed_factor; - block->nominal_rate *= speed_factor; + TERN_(HAS_STANDARD_MOTION, block->nominal_rate *= speed_factor); block->nominal_speed *= speed_factor; } @@ -2409,11 +2426,13 @@ bool Planner::_populate_block( ); } } - block->acceleration_steps_per_s2 = accel; - block->acceleration = accel / steps_per_mm; - #if DISABLED(S_CURVE_ACCELERATION) - block->acceleration_rate = uint32_t(accel * (float(_BV32(24)) / (STEPPER_TIMER_RATE))); + #if HAS_STANDARD_MOTION + block->acceleration_steps_per_s2 = accel; + #if DISABLED(S_CURVE_ACCELERATION) + block->acceleration_rate = uint32_t(accel * (float(_BV32(24)) / (STEPPER_TIMER_RATE))); + #endif #endif + block->acceleration = accel / steps_per_mm; #if HAS_ROUGH_LIN_ADVANCE block->la_advance_rate = 0; @@ -2724,8 +2743,10 @@ bool Planner::_populate_block( block->entry_speed_sqr = minimum_planner_speed_sqr; // Set min entry speed. Rarely it could be higher than the previous nominal speed but that's ok. block->min_entry_speed_sqr = minimum_planner_speed_sqr; - // Zero the initial_rate to indicate that calculate_trapezoid_for_block() hasn't been called yet. - block->initial_rate = 0; + + // Zero initial_rate and/or entry_speed to indicate calculate_trapezoid_for_block() needs a call. + TERN_(HAS_STANDARD_MOTION, block->initial_rate = 0); + TERN_(FT_MOTION, block->entry_speed = 0); block->flag.recalculate = true; diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index 2afec3b4ab..2838fb90c3 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -254,7 +254,7 @@ typedef struct PlannerBlock { #if ENABLED(S_CURVE_ACCELERATION) uint32_t acceleration_time_inverse, // Inverse of acceleration and deceleration periods, expressed as integer. Scale depends on CPU being used deceleration_time_inverse; - #else + #elif HAS_STANDARD_MOTION uint32_t acceleration_rate; // Acceleration rate in (2^24 steps)/timer_ticks*s #endif @@ -277,10 +277,17 @@ typedef struct PlannerBlock { #endif #endif - uint32_t nominal_rate, // The nominal step rate for this block in step_events/sec - initial_rate, // The jerk-adjusted step rate at start of block - final_rate, // The minimal rate at exit - acceleration_steps_per_s2; // acceleration steps/sec^2 + #if ENABLED(FT_MOTION) + float entry_speed, // Block entry speed in steps units + exit_speed; // Block exit speed in steps units + #endif + + #if HAS_STANDARD_MOTION + uint32_t nominal_rate, // The nominal step rate for this block in step_events/sec + initial_rate, // The jerk-adjusted step rate at start of block + final_rate, // The minimal rate at exit + acceleration_steps_per_s2; // acceleration steps/sec^2 + #endif #if ENABLED(DIRECT_STEPPING) page_idx_t page_idx; // Page index used for direct stepping diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index d79a5edcc0..947a74f96a 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -138,8 +138,10 @@ stepper_flags_t Stepper::axis_enabled; // {0} block_t* Stepper::current_block; // (= nullptr) A pointer to the block currently being traced -AxisBits Stepper::last_direction_bits, // = 0 - Stepper::axis_did_move; // = 0 +#if HAS_STANDARD_MOTION + AxisBits Stepper::axis_did_move; // = 0 +#endif +AxisBits Stepper::last_direction_bits; // = 0 bool Stepper::abort_current_block; @@ -516,6 +518,9 @@ xyze_int8_t Stepper::count_direction{0}; #define W_APPLY_STEP(STATE,Q) W_STEP_WRITE(STATE) #endif +#define _APPLY_STEP(AXIS, STATE, ALWAYS) AXIS ##_APPLY_STEP(STATE, ALWAYS) +#define _STEP_STATE(AXIS) STEP_STATE_## AXIS + //#define E0_APPLY_DIR(FWD) do{ (FWD) ? FWD_E_DIR(0) : REV_E_DIR(0); }while(0) //#define E1_APPLY_DIR(FWD) do{ (FWD) ? FWD_E_DIR(1) : REV_E_DIR(1); }while(0) //#define E2_APPLY_DIR(FWD) do{ (FWD) ? FWD_E_DIR(2) : REV_E_DIR(2); }while(0) @@ -1583,11 +1588,11 @@ void Stepper::isr() { #if ENABLED(FT_MOTION) static uint32_t ftMotion_nextStepperISR = 0U; // Storage for the next ISR for stepping. - const bool using_ftMotion = ftMotion.cfg.active; - #else - constexpr bool using_ftMotion = false; #endif + // FT Motion can be toggled if Standard Motion is also active + const bool using_ftMotion = ENABLED(NO_STANDARD_MOTION) || TERN0(FT_MOTION, ftMotion.cfg.active); + // We need this variable here to be able to use it in the following loop hal_timer_t min_ticks; do { @@ -1628,72 +1633,76 @@ void Stepper::isr() { ftMotion_nextStepperISR -= interval; } - #endif + #endif // FT_MOTION - if (!using_ftMotion) { + #if HAS_STANDARD_MOTION - TERN_(HAS_ZV_SHAPING, shaping_isr()); // Do Shaper stepping, if needed + if (!using_ftMotion) { - if (!nextMainISR) pulse_phase_isr(); // 0 = Do coordinated axes Stepper pulses + TERN_(HAS_ZV_SHAPING, shaping_isr()); // Do Shaper stepping, if needed - #if ENABLED(LIN_ADVANCE) - if (!nextAdvanceISR) { // 0 = Do Linear Advance E Stepper pulses - advance_isr(); - nextAdvanceISR = la_interval; - } - else if (nextAdvanceISR > la_interval) // Start/accelerate LA steps if necessary - nextAdvanceISR = la_interval; - #endif + if (!nextMainISR) pulse_phase_isr(); // 0 = Do coordinated axes Stepper pulses - #if ENABLED(BABYSTEPPING) - // Time to run babystepping and apply STEP/DIR pulses? - // babystepping_isr -> babystep.task -> [ babystep.step_axis(*) -> stepper.do_babystep ] - const bool is_babystep = (nextBabystepISR == 0); // 0 = Do Babystepping (XY)Z pulses - if (is_babystep) nextBabystepISR = babystepping_isr(); - #endif + #if ENABLED(LIN_ADVANCE) + if (!nextAdvanceISR) { // 0 = Do Linear Advance E Stepper pulses + advance_isr(); + nextAdvanceISR = la_interval; + } + else if (nextAdvanceISR > la_interval) // Start/accelerate LA steps if necessary + nextAdvanceISR = la_interval; + #endif - // Enable ISRs to reduce latency for higher priority ISRs, or all ISRs if no prioritization. - hal.isr_on(); + #if ENABLED(BABYSTEPPING) + // Time to run babystepping and apply STEP/DIR pulses? + // babystepping_isr -> babystep.task -> [ babystep.step_axis(*) -> stepper.do_babystep ] + const bool is_babystep = (nextBabystepISR == 0); // 0 = Do Babystepping (XY)Z pulses + if (is_babystep) nextBabystepISR = babystepping_isr(); + #endif - // ^== Time critical. NOTHING besides pulse generation should be above here!!! + // Enable ISRs to reduce latency for higher priority ISRs, or all ISRs if no prioritization. + hal.isr_on(); - if (!nextMainISR) nextMainISR = block_phase_isr(); // Manage acc/deceleration, get next block - #if ENABLED(SMOOTH_LIN_ADVANCE) - if (!smoothLinAdvISR) smoothLinAdvISR = smooth_lin_adv_isr(); // Manage la - #endif + // ^== Time critical. NOTHING besides pulse generation should be above here!!! - #if ENABLED(BABYSTEPPING) - if (is_babystep) // Avoid ANY stepping too soon after baby-stepping - NOLESS(nextMainISR, (BABYSTEP_TICKS) / 8); // FULL STOP for 125µs after a baby-step + if (!nextMainISR) nextMainISR = block_phase_isr(); // Manage acc/deceleration, get next block + #if ENABLED(SMOOTH_LIN_ADVANCE) + if (!smoothLinAdvISR) smoothLinAdvISR = smooth_lin_adv_isr(); // Manage la + #endif - if (nextBabystepISR != BABYSTEP_NEVER) // Avoid baby-stepping too close to axis Stepping - NOLESS(nextBabystepISR, nextMainISR / 2); // TODO: Only look at axes enabled for baby-stepping - #endif + #if ENABLED(BABYSTEPPING) + if (is_babystep) // Avoid ANY stepping too soon after baby-stepping + NOLESS(nextMainISR, (BABYSTEP_TICKS) / 8); // FULL STOP for 125µs after a baby-step - // Get the interval to the next ISR call - interval = hal_timer_t(STEPPER_TIMER_RATE * 0.03); // Max wait of 30ms regardless of stepper timer frequency - NOMORE(interval, nextMainISR); // Time until the next Pulse / Block phase - TERN_(INPUT_SHAPING_X, NOMORE(interval, ShapingQueue::peek_x())); // Time until next input shaping echo for X - TERN_(INPUT_SHAPING_Y, NOMORE(interval, ShapingQueue::peek_y())); // Time until next input shaping echo for Y - TERN_(INPUT_SHAPING_Z, NOMORE(interval, ShapingQueue::peek_z())); // Time until next input shaping echo for Z - TERN_(LIN_ADVANCE, NOMORE(interval, nextAdvanceISR)); // Come back early for Linear Advance? - TERN_(SMOOTH_LIN_ADVANCE, NOMORE(interval, smoothLinAdvISR)); // Come back early for Linear Advance rate update? - TERN_(BABYSTEPPING, NOMORE(interval, nextBabystepISR)); // Come back early for Babystepping? + if (nextBabystepISR != BABYSTEP_NEVER) // Avoid baby-stepping too close to axis Stepping + NOLESS(nextBabystepISR, nextMainISR / 2); // TODO: Only look at axes enabled for baby-stepping + #endif - // - // Compute remaining time for each ISR phase - // NEVER : The phase is idle - // Zero : The phase will occur on the next ISR call - // Non-zero : The phase will occur on a future ISR call - // + // Get the interval to the next ISR call + interval = hal_timer_t(STEPPER_TIMER_RATE * 0.03); // Max wait of 30ms regardless of stepper timer frequency + NOMORE(interval, nextMainISR); // Time until the next Pulse / Block phase + TERN_(INPUT_SHAPING_X, NOMORE(interval, ShapingQueue::peek_x())); // Time until next input shaping echo for X + TERN_(INPUT_SHAPING_Y, NOMORE(interval, ShapingQueue::peek_y())); // Time until next input shaping echo for Y + TERN_(INPUT_SHAPING_Z, NOMORE(interval, ShapingQueue::peek_z())); // Time until next input shaping echo for Z + TERN_(LIN_ADVANCE, NOMORE(interval, nextAdvanceISR)); // Come back early for Linear Advance? + TERN_(SMOOTH_LIN_ADVANCE, NOMORE(interval, smoothLinAdvISR)); // Come back early for Linear Advance rate update? + TERN_(BABYSTEPPING, NOMORE(interval, nextBabystepISR)); // Come back early for Babystepping? - nextMainISR -= interval; - TERN_(HAS_ZV_SHAPING, ShapingQueue::decrement_delays(interval)); - TERN_(LIN_ADVANCE, if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval); - TERN_(SMOOTH_LIN_ADVANCE, if (smoothLinAdvISR != LA_ADV_NEVER) smoothLinAdvISR -= interval); - TERN_(BABYSTEPPING, if (nextBabystepISR != BABYSTEP_NEVER) nextBabystepISR -= interval); + // + // Compute remaining time for each ISR phase + // NEVER : The phase is idle + // Zero : The phase will occur on the next ISR call + // Non-zero : The phase will occur on a future ISR call + // - } // standard motion control + nextMainISR -= interval; + TERN_(HAS_ZV_SHAPING, ShapingQueue::decrement_delays(interval)); + TERN_(LIN_ADVANCE, if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval); + TERN_(SMOOTH_LIN_ADVANCE, if (smoothLinAdvISR != LA_ADV_NEVER) smoothLinAdvISR -= interval); + TERN_(BABYSTEPPING, if (nextBabystepISR != BABYSTEP_NEVER) nextBabystepISR -= interval); + + } + + #endif // HAS_STANDARD_MOTION /** * This needs to avoid a race-condition caused by interleaving @@ -1798,335 +1807,335 @@ void Stepper::isr() { #define ISR_MULTI_STEPS 1 #endif -/** - * This phase of the ISR should ONLY create the pulses for the steppers. - * This prevents jitter caused by the interval between the start of the - * interrupt and the start of the pulses. DON'T add any logic ahead of the - * call to this method that might cause variation in the timing. The aim - * is to keep pulse timing as regular as possible. - */ -void Stepper::pulse_phase_isr() { +#if HAS_STANDARD_MOTION + /** + * This phase of the ISR should ONLY create the pulses for the steppers. + * This prevents jitter caused by the interval between the start of the + * interrupt and the start of the pulses. DON'T add any logic ahead of the + * call to this method that might cause variation in the timing. The aim + * is to keep pulse timing as regular as possible. + */ + void Stepper::pulse_phase_isr() { - // If we must abort the current block, do so! - if (abort_current_block) { - abort_current_block = false; - if (current_block) { - discard_current_block(); - #if HAS_ZV_SHAPING - ShapingQueue::purge(); - #if ENABLED(INPUT_SHAPING_X) - shaping_x.delta_error = 0; - shaping_x.last_block_end_pos = count_position.x; + // If we must abort the current block, do so! + if (abort_current_block) { + abort_current_block = false; + if (current_block) { + discard_current_block(); + #if HAS_ZV_SHAPING + ShapingQueue::purge(); + #if ENABLED(INPUT_SHAPING_X) + shaping_x.delta_error = 0; + shaping_x.last_block_end_pos = count_position.x; + #endif + #if ENABLED(INPUT_SHAPING_Y) + shaping_y.delta_error = 0; + shaping_y.last_block_end_pos = count_position.y; + #endif + #if ENABLED(INPUT_SHAPING_Z) + shaping_z.delta_error = 0; + shaping_z.last_block_end_pos = count_position.z; + #endif #endif - #if ENABLED(INPUT_SHAPING_Y) - shaping_y.delta_error = 0; - shaping_y.last_block_end_pos = count_position.y; - #endif - #if ENABLED(INPUT_SHAPING_Z) - shaping_z.delta_error = 0; - shaping_z.last_block_end_pos = count_position.z; - #endif - #endif + } } + + // If there is no current block, do nothing + if (!current_block || step_events_completed >= step_event_count) return; + + // Skipping step processing causes motion to freeze + if (TERN0(FREEZE_FEATURE, frozen)) return; + + // Count of pending loops and events for this iteration + const uint32_t pending_events = step_event_count - step_events_completed; + uint8_t events_to_do = _MIN(pending_events, steps_per_isr); + + // Just update the value we will get at the end of the loop + step_events_completed += events_to_do; + + TERN_(ISR_PULSE_CONTROL, USING_TIMED_PULSE()); + + // Take multiple steps per interrupt. For high speed moves. + #if ENABLED(ISR_MULTI_STEPS) + bool firstStep = true; + #endif + + // Direct Stepping page? + const bool is_page = current_block->is_page(); + + do { + AxisFlags step_needed{0}; + + // Determine if a pulse is needed using Bresenham + #define PULSE_PREP(AXIS) do{ \ + int32_t de = delta_error[_AXIS(AXIS)] + advance_dividend[_AXIS(AXIS)]; \ + if (de >= 0) { \ + step_needed.set(_AXIS(AXIS)); \ + de -= advance_divisor_cached; \ + } \ + delta_error[_AXIS(AXIS)] = de; \ + }while(0) + + // With input shaping, direction changes can happen with almost only + // AWAIT_LOW_PULSE() and DIR_WAIT_BEFORE() between steps. To work around + // the TMC2208 / TMC2225 shutdown bug (#16076), add a half step hysteresis + // in each direction. This results in the position being off by half an + // average half step during travel but correct at the end of each segment. + #if AXIS_DRIVER_TYPE_X(TMC2208) || AXIS_DRIVER_TYPE_X(TMC2208_STANDALONE) || \ + AXIS_DRIVER_TYPE_X(TMC5160) || AXIS_DRIVER_TYPE_X(TMC5160_STANDALONE) + #define HYSTERESIS_X 64 + #else + #define HYSTERESIS_X 0 + #endif + #if AXIS_DRIVER_TYPE_Y(TMC2208) || AXIS_DRIVER_TYPE_Y(TMC2208_STANDALONE) || \ + AXIS_DRIVER_TYPE_Y(TMC5160) || AXIS_DRIVER_TYPE_Y(TMC5160_STANDALONE) + #define HYSTERESIS_Y 64 + #else + #define HYSTERESIS_Y 0 + #endif + #if AXIS_DRIVER_TYPE_Z(TMC2208) || AXIS_DRIVER_TYPE_Z(TMC2208_STANDALONE) || \ + AXIS_DRIVER_TYPE_Z(TMC5160) || AXIS_DRIVER_TYPE_Z(TMC5160_STANDALONE) + #define HYSTERESIS_Z 64 + #else + #define HYSTERESIS_Z 0 + #endif + #define _HYSTERESIS(AXIS) HYSTERESIS_##AXIS + #define HYSTERESIS(AXIS) _HYSTERESIS(AXIS) + + #define PULSE_PREP_SHAPING(AXIS, DELTA_ERROR, DIVIDEND) do{ \ + int16_t de = DELTA_ERROR + (DIVIDEND); \ + const bool step_fwd = de >= (64 + HYSTERESIS(AXIS)), \ + step_bak = de <= -(64 + HYSTERESIS(AXIS)); \ + if (step_fwd || step_bak) { \ + de += step_fwd ? -128 : 128; \ + if ((MAXDIR(AXIS) && step_bak) || (MINDIR(AXIS) && step_fwd)) { \ + { USING_TIMED_PULSE(); START_TIMED_PULSE(); AWAIT_LOW_PULSE(); } \ + last_direction_bits.toggle(_AXIS(AXIS)); \ + DIR_WAIT_BEFORE(); \ + SET_STEP_DIR(AXIS); \ + TERN_(FT_MOTION, last_set_direction = last_direction_bits); \ + DIR_WAIT_AFTER(); \ + } \ + } \ + else \ + step_needed.clear(_AXIS(AXIS)); \ + DELTA_ERROR = de; \ + }while(0) + + // Start an active pulse if needed + #define PULSE_START(AXIS) do{ \ + if (step_needed.test(_AXIS(AXIS))) { \ + count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \ + _APPLY_STEP(AXIS, _STEP_STATE(AXIS), false); \ + } \ + }while(0) + + // Stop an active pulse if needed + #define PULSE_STOP(AXIS) do { \ + if (step_needed.test(_AXIS(AXIS))) { \ + _APPLY_STEP(AXIS, !_STEP_STATE(AXIS), false); \ + } \ + }while(0) + + #if ENABLED(DIRECT_STEPPING) + // Direct stepping is currently not ready for HAS_I_AXIS + if (is_page) { + + #if STEPPER_PAGE_FORMAT == SP_4x4D_128 + + #define PAGE_SEGMENT_UPDATE(AXIS, VALUE) do{ \ + if ((VALUE) < 7) dm[_AXIS(AXIS)] = false; \ + else if ((VALUE) > 7) dm[_AXIS(AXIS)] = true; \ + page_step_state.sd[_AXIS(AXIS)] = VALUE; \ + page_step_state.bd[_AXIS(AXIS)] += VALUE; \ + }while(0) + + #define PAGE_PULSE_PREP(AXIS) do{ \ + step_needed.set(_AXIS(AXIS), \ + pgm_read_byte(&segment_table[page_step_state.sd[_AXIS(AXIS)]][page_step_state.segment_steps & 0x7])); \ + }while(0) + + switch (page_step_state.segment_steps) { + case DirectStepping::Config::SEGMENT_STEPS: + page_step_state.segment_idx += 2; + page_step_state.segment_steps = 0; + // fallthru + case 0: { + const uint8_t low = page_step_state.page[page_step_state.segment_idx], + high = page_step_state.page[page_step_state.segment_idx + 1]; + const AxisBits dm = last_direction_bits; + + PAGE_SEGMENT_UPDATE(X, low >> 4); + PAGE_SEGMENT_UPDATE(Y, low & 0xF); + PAGE_SEGMENT_UPDATE(Z, high >> 4); + PAGE_SEGMENT_UPDATE(E, high & 0xF); + + if (dm != last_direction_bits) set_directions(dm); + + } break; + + default: break; + } + + PAGE_PULSE_PREP(X); + PAGE_PULSE_PREP(Y); + PAGE_PULSE_PREP(Z); + TERN_(HAS_EXTRUDERS, PAGE_PULSE_PREP(E)); + + page_step_state.segment_steps++; + + #elif STEPPER_PAGE_FORMAT == SP_4x2_256 + + #define PAGE_SEGMENT_UPDATE(AXIS, VALUE) \ + page_step_state.sd[_AXIS(AXIS)] = VALUE; \ + page_step_state.bd[_AXIS(AXIS)] += VALUE; + + #define PAGE_PULSE_PREP(AXIS) do{ \ + step_needed.set(_AXIS(AXIS), \ + pgm_read_byte(&segment_table[page_step_state.sd[_AXIS(AXIS)]][page_step_state.segment_steps & 0x3])); \ + }while(0) + + switch (page_step_state.segment_steps) { + case DirectStepping::Config::SEGMENT_STEPS: + page_step_state.segment_idx++; + page_step_state.segment_steps = 0; + // fallthru + case 0: { + const uint8_t b = page_step_state.page[page_step_state.segment_idx]; + PAGE_SEGMENT_UPDATE(X, (b >> 6) & 0x3); + PAGE_SEGMENT_UPDATE(Y, (b >> 4) & 0x3); + PAGE_SEGMENT_UPDATE(Z, (b >> 2) & 0x3); + PAGE_SEGMENT_UPDATE(E, (b >> 0) & 0x3); + } break; + default: break; + } + + PAGE_PULSE_PREP(X); + PAGE_PULSE_PREP(Y); + PAGE_PULSE_PREP(Z); + TERN_(HAS_EXTRUDERS, PAGE_PULSE_PREP(E)); + + page_step_state.segment_steps++; + + #elif STEPPER_PAGE_FORMAT == SP_4x1_512 + + #define PAGE_PULSE_PREP(AXIS, NBIT) do{ \ + step_needed.set(_AXIS(AXIS), TEST(steps, NBIT)); \ + if (step_needed.test(_AXIS(AXIS))) \ + page_step_state.bd[_AXIS(AXIS)]++; \ + }while(0) + + uint8_t steps = page_step_state.page[page_step_state.segment_idx >> 1]; + if (page_step_state.segment_idx & 0x1) steps >>= 4; + + PAGE_PULSE_PREP(X, 3); + PAGE_PULSE_PREP(Y, 2); + PAGE_PULSE_PREP(Z, 1); + PAGE_PULSE_PREP(E, 0); + + page_step_state.segment_idx++; + + #else + #error "Unknown direct stepping page format!" + #endif + } + + #endif // DIRECT_STEPPING + + if (!is_page) { + // Give the compiler a clue to store advance_divisor in registers for what follows + const uint32_t advance_divisor_cached = advance_divisor; + + // Determine if pulses are needed + #define _PULSE_PREP(A) TERF(HAS_##A##_STEP, PULSE_PREP)(A); + MAIN_AXIS_MAP(_PULSE_PREP); + + #if ANY(HAS_E0_STEP, MIXING_EXTRUDER) + PULSE_PREP(E); + #endif + + #if HAS_ROUGH_LIN_ADVANCE + if (la_active && step_needed.e) { + // Don't actually step here, but do subtract movements steps + // from the linear advance step count + step_needed.e = false; + la_advance_steps--; + } + #elif ENABLED(SMOOTH_LIN_ADVANCE) + // Extruder steps are exclusively managed by the LA isr + step_needed.e = false; + #endif + + #if HAS_ZV_SHAPING + // Record an echo if a step is needed in the primary Bresenham + const bool x_step = TERN0(INPUT_SHAPING_X, step_needed.x && shaping_x.enabled), + y_step = TERN0(INPUT_SHAPING_Y, step_needed.y && shaping_y.enabled), + z_step = TERN0(INPUT_SHAPING_Z, step_needed.z && shaping_z.enabled); + if (x_step || y_step || z_step) + ShapingQueue::enqueue(x_step, TERN0(INPUT_SHAPING_X, shaping_x.forward), y_step, TERN0(INPUT_SHAPING_Y, shaping_y.forward), z_step, TERN0(INPUT_SHAPING_Z, shaping_z.forward)); + + // Do the first part of the secondary Bresenham + #if ENABLED(INPUT_SHAPING_X) + if (x_step) + PULSE_PREP_SHAPING(X, shaping_x.delta_error, shaping_x.forward ? shaping_x.factor1 : -shaping_x.factor1); + #endif + #if ENABLED(INPUT_SHAPING_Y) + if (y_step) + PULSE_PREP_SHAPING(Y, shaping_y.delta_error, shaping_y.forward ? shaping_y.factor1 : -shaping_y.factor1); + #endif + #if ENABLED(INPUT_SHAPING_Z) + if (z_step) + PULSE_PREP_SHAPING(Z, shaping_z.delta_error, shaping_z.forward ? shaping_z.factor1 : -shaping_z.factor1); + #endif + #endif + } + + #if ISR_MULTI_STEPS + if (firstStep) + firstStep = false; + else + AWAIT_LOW_PULSE(); + #endif + + // Pulse start + #define _PULSE_START(A) TERF(HAS_##A##_STEP, PULSE_START)(A); + MAIN_AXIS_MAP(_PULSE_START); + + #if ENABLED(MIXING_EXTRUDER) + if (step_needed.e) { + count_position.e += count_direction.e; + E_STEP_WRITE(mixer.get_next_stepper(), STEP_STATE_E); + } + #elif HAS_E0_STEP + PULSE_START(E); + #endif + + TERN_(I2S_STEPPER_STREAM, i2s_push_sample()); + + // TODO: need to deal with MINIMUM_STEPPER_PULSE_NS over i2s + #if ISR_PULSE_CONTROL + START_TIMED_PULSE(); + AWAIT_HIGH_PULSE(); + #endif + + // Pulse stop + #define _PULSE_STOP(A) TERF(HAS_##A##_STEP, PULSE_STOP)(A); + MAIN_AXIS_MAP(_PULSE_STOP); + + #if ENABLED(MIXING_EXTRUDER) + if (step_needed.e) E_STEP_WRITE(mixer.get_stepper(), !STEP_STATE_E); + #elif HAS_E0_STEP + PULSE_STOP(E); + #endif + + #if ISR_MULTI_STEPS + if (events_to_do) START_TIMED_PULSE(); + #endif + + } while (--events_to_do); } - // If there is no current block, do nothing - if (!current_block || step_events_completed >= step_event_count) return; - - // Skipping step processing causes motion to freeze - if (TERN0(FREEZE_FEATURE, frozen)) return; - - // Count of pending loops and events for this iteration - const uint32_t pending_events = step_event_count - step_events_completed; - uint8_t events_to_do = _MIN(pending_events, steps_per_isr); - - // Just update the value we will get at the end of the loop - step_events_completed += events_to_do; - - TERN_(ISR_PULSE_CONTROL, USING_TIMED_PULSE()); - - // Take multiple steps per interrupt. For high speed moves. - #if ENABLED(ISR_MULTI_STEPS) - bool firstStep = true; - #endif - - // Direct Stepping page? - const bool is_page = current_block->is_page(); - - do { - AxisFlags step_needed{0}; - - #define _APPLY_STEP(AXIS, STATE, ALWAYS) AXIS ##_APPLY_STEP(STATE, ALWAYS) - #define _STEP_STATE(AXIS) STEP_STATE_## AXIS - - // Determine if a pulse is needed using Bresenham - #define PULSE_PREP(AXIS) do{ \ - int32_t de = delta_error[_AXIS(AXIS)] + advance_dividend[_AXIS(AXIS)]; \ - if (de >= 0) { \ - step_needed.set(_AXIS(AXIS)); \ - de -= advance_divisor_cached; \ - } \ - delta_error[_AXIS(AXIS)] = de; \ - }while(0) - - // With input shaping, direction changes can happen with almost only - // AWAIT_LOW_PULSE() and DIR_WAIT_BEFORE() between steps. To work around - // the TMC2208 / TMC2225 shutdown bug (#16076), add a half step hysteresis - // in each direction. This results in the position being off by half an - // average half step during travel but correct at the end of each segment. - #if AXIS_DRIVER_TYPE_X(TMC2208) || AXIS_DRIVER_TYPE_X(TMC2208_STANDALONE) || \ - AXIS_DRIVER_TYPE_X(TMC5160) || AXIS_DRIVER_TYPE_X(TMC5160_STANDALONE) - #define HYSTERESIS_X 64 - #else - #define HYSTERESIS_X 0 - #endif - #if AXIS_DRIVER_TYPE_Y(TMC2208) || AXIS_DRIVER_TYPE_Y(TMC2208_STANDALONE) || \ - AXIS_DRIVER_TYPE_Y(TMC5160) || AXIS_DRIVER_TYPE_Y(TMC5160_STANDALONE) - #define HYSTERESIS_Y 64 - #else - #define HYSTERESIS_Y 0 - #endif - #if AXIS_DRIVER_TYPE_Z(TMC2208) || AXIS_DRIVER_TYPE_Z(TMC2208_STANDALONE) || \ - AXIS_DRIVER_TYPE_Z(TMC5160) || AXIS_DRIVER_TYPE_Z(TMC5160_STANDALONE) - #define HYSTERESIS_Z 64 - #else - #define HYSTERESIS_Z 0 - #endif - #define _HYSTERESIS(AXIS) HYSTERESIS_##AXIS - #define HYSTERESIS(AXIS) _HYSTERESIS(AXIS) - - #define PULSE_PREP_SHAPING(AXIS, DELTA_ERROR, DIVIDEND) do{ \ - int16_t de = DELTA_ERROR + (DIVIDEND); \ - const bool step_fwd = de >= (64 + HYSTERESIS(AXIS)), \ - step_bak = de <= -(64 + HYSTERESIS(AXIS)); \ - if (step_fwd || step_bak) { \ - de += step_fwd ? -128 : 128; \ - if ((MAXDIR(AXIS) && step_bak) || (MINDIR(AXIS) && step_fwd)) { \ - { USING_TIMED_PULSE(); START_TIMED_PULSE(); AWAIT_LOW_PULSE(); } \ - last_direction_bits.toggle(_AXIS(AXIS)); \ - DIR_WAIT_BEFORE(); \ - SET_STEP_DIR(AXIS); \ - TERN_(FT_MOTION, last_set_direction = last_direction_bits); \ - DIR_WAIT_AFTER(); \ - } \ - } \ - else \ - step_needed.clear(_AXIS(AXIS)); \ - DELTA_ERROR = de; \ - }while(0) - - // Start an active pulse if needed - #define PULSE_START(AXIS) do{ \ - if (step_needed.test(_AXIS(AXIS))) { \ - count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \ - _APPLY_STEP(AXIS, _STEP_STATE(AXIS), 0); \ - } \ - }while(0) - - // Stop an active pulse if needed - #define PULSE_STOP(AXIS) do { \ - if (step_needed.test(_AXIS(AXIS))) { \ - _APPLY_STEP(AXIS, !_STEP_STATE(AXIS), 0); \ - } \ - }while(0) - - #if ENABLED(DIRECT_STEPPING) - // Direct stepping is currently not ready for HAS_I_AXIS - if (is_page) { - - #if STEPPER_PAGE_FORMAT == SP_4x4D_128 - - #define PAGE_SEGMENT_UPDATE(AXIS, VALUE) do{ \ - if ((VALUE) < 7) dm[_AXIS(AXIS)] = false; \ - else if ((VALUE) > 7) dm[_AXIS(AXIS)] = true; \ - page_step_state.sd[_AXIS(AXIS)] = VALUE; \ - page_step_state.bd[_AXIS(AXIS)] += VALUE; \ - }while(0) - - #define PAGE_PULSE_PREP(AXIS) do{ \ - step_needed.set(_AXIS(AXIS), \ - pgm_read_byte(&segment_table[page_step_state.sd[_AXIS(AXIS)]][page_step_state.segment_steps & 0x7])); \ - }while(0) - - switch (page_step_state.segment_steps) { - case DirectStepping::Config::SEGMENT_STEPS: - page_step_state.segment_idx += 2; - page_step_state.segment_steps = 0; - // fallthru - case 0: { - const uint8_t low = page_step_state.page[page_step_state.segment_idx], - high = page_step_state.page[page_step_state.segment_idx + 1]; - const AxisBits dm = last_direction_bits; - - PAGE_SEGMENT_UPDATE(X, low >> 4); - PAGE_SEGMENT_UPDATE(Y, low & 0xF); - PAGE_SEGMENT_UPDATE(Z, high >> 4); - PAGE_SEGMENT_UPDATE(E, high & 0xF); - - if (dm != last_direction_bits) set_directions(dm); - - } break; - - default: break; - } - - PAGE_PULSE_PREP(X); - PAGE_PULSE_PREP(Y); - PAGE_PULSE_PREP(Z); - TERN_(HAS_EXTRUDERS, PAGE_PULSE_PREP(E)); - - page_step_state.segment_steps++; - - #elif STEPPER_PAGE_FORMAT == SP_4x2_256 - - #define PAGE_SEGMENT_UPDATE(AXIS, VALUE) \ - page_step_state.sd[_AXIS(AXIS)] = VALUE; \ - page_step_state.bd[_AXIS(AXIS)] += VALUE; - - #define PAGE_PULSE_PREP(AXIS) do{ \ - step_needed.set(_AXIS(AXIS), \ - pgm_read_byte(&segment_table[page_step_state.sd[_AXIS(AXIS)]][page_step_state.segment_steps & 0x3])); \ - }while(0) - - switch (page_step_state.segment_steps) { - case DirectStepping::Config::SEGMENT_STEPS: - page_step_state.segment_idx++; - page_step_state.segment_steps = 0; - // fallthru - case 0: { - const uint8_t b = page_step_state.page[page_step_state.segment_idx]; - PAGE_SEGMENT_UPDATE(X, (b >> 6) & 0x3); - PAGE_SEGMENT_UPDATE(Y, (b >> 4) & 0x3); - PAGE_SEGMENT_UPDATE(Z, (b >> 2) & 0x3); - PAGE_SEGMENT_UPDATE(E, (b >> 0) & 0x3); - } break; - default: break; - } - - PAGE_PULSE_PREP(X); - PAGE_PULSE_PREP(Y); - PAGE_PULSE_PREP(Z); - TERN_(HAS_EXTRUDERS, PAGE_PULSE_PREP(E)); - - page_step_state.segment_steps++; - - #elif STEPPER_PAGE_FORMAT == SP_4x1_512 - - #define PAGE_PULSE_PREP(AXIS, NBIT) do{ \ - step_needed.set(_AXIS(AXIS), TEST(steps, NBIT)); \ - if (step_needed.test(_AXIS(AXIS))) \ - page_step_state.bd[_AXIS(AXIS)]++; \ - }while(0) - - uint8_t steps = page_step_state.page[page_step_state.segment_idx >> 1]; - if (page_step_state.segment_idx & 0x1) steps >>= 4; - - PAGE_PULSE_PREP(X, 3); - PAGE_PULSE_PREP(Y, 2); - PAGE_PULSE_PREP(Z, 1); - PAGE_PULSE_PREP(E, 0); - - page_step_state.segment_idx++; - - #else - #error "Unknown direct stepping page format!" - #endif - } - - #endif // DIRECT_STEPPING - - if (!is_page) { - // Give the compiler a clue to store advance_divisor in registers for what follows - const uint32_t advance_divisor_cached = advance_divisor; - - // Determine if pulses are needed - #define _PULSE_PREP(A) TERF(HAS_##A##_STEP, PULSE_PREP)(A); - MAIN_AXIS_MAP(_PULSE_PREP); - - #if ANY(HAS_E0_STEP, MIXING_EXTRUDER) - PULSE_PREP(E); - #endif - - #if HAS_ROUGH_LIN_ADVANCE - if (la_active && step_needed.e) { - // Don't actually step here, but do subtract movements steps - // from the linear advance step count - step_needed.e = false; - la_advance_steps--; - } - #elif ENABLED(SMOOTH_LIN_ADVANCE) - // Extruder steps are exclusively managed by the LA isr - step_needed.e = false; - #endif - - #if HAS_ZV_SHAPING - // Record an echo if a step is needed in the primary Bresenham - const bool x_step = TERN0(INPUT_SHAPING_X, step_needed.x && shaping_x.enabled), - y_step = TERN0(INPUT_SHAPING_Y, step_needed.y && shaping_y.enabled), - z_step = TERN0(INPUT_SHAPING_Z, step_needed.z && shaping_z.enabled); - if (x_step || y_step || z_step) - ShapingQueue::enqueue(x_step, TERN0(INPUT_SHAPING_X, shaping_x.forward), y_step, TERN0(INPUT_SHAPING_Y, shaping_y.forward), z_step, TERN0(INPUT_SHAPING_Z, shaping_z.forward)); - - // Do the first part of the secondary Bresenham - #if ENABLED(INPUT_SHAPING_X) - if (x_step) - PULSE_PREP_SHAPING(X, shaping_x.delta_error, shaping_x.forward ? shaping_x.factor1 : -shaping_x.factor1); - #endif - #if ENABLED(INPUT_SHAPING_Y) - if (y_step) - PULSE_PREP_SHAPING(Y, shaping_y.delta_error, shaping_y.forward ? shaping_y.factor1 : -shaping_y.factor1); - #endif - #if ENABLED(INPUT_SHAPING_Z) - if (z_step) - PULSE_PREP_SHAPING(Z, shaping_z.delta_error, shaping_z.forward ? shaping_z.factor1 : -shaping_z.factor1); - #endif - #endif - } - - #if ISR_MULTI_STEPS - if (firstStep) - firstStep = false; - else - AWAIT_LOW_PULSE(); - #endif - - // Pulse start - #define _PULSE_START(A) TERF(HAS_##A##_STEP, PULSE_START)(A); - MAIN_AXIS_MAP(_PULSE_START); - - #if ENABLED(MIXING_EXTRUDER) - if (step_needed.e) { - count_position.e += count_direction.e; - E_STEP_WRITE(mixer.get_next_stepper(), STEP_STATE_E); - } - #elif HAS_E0_STEP - PULSE_START(E); - #endif - - TERN_(I2S_STEPPER_STREAM, i2s_push_sample()); - - // TODO: need to deal with MINIMUM_STEPPER_PULSE_NS over i2s - #if ISR_PULSE_CONTROL - START_TIMED_PULSE(); - AWAIT_HIGH_PULSE(); - #endif - - // Pulse stop - #define _PULSE_STOP(A) TERF(HAS_##A##_STEP, PULSE_STOP)(A); - MAIN_AXIS_MAP(_PULSE_STOP); - - #if ENABLED(MIXING_EXTRUDER) - if (step_needed.e) E_STEP_WRITE(mixer.get_stepper(), !STEP_STATE_E); - #elif HAS_E0_STEP - PULSE_STOP(E); - #endif - - #if ISR_MULTI_STEPS - if (events_to_do) START_TIMED_PULSE(); - #endif - - } while (--events_to_do); -} +#endif // HAS_STANDARD_MOTION #if HAS_ZV_SHAPING @@ -2195,737 +2204,742 @@ void Stepper::pulse_phase_isr() { #endif // HAS_ZV_SHAPING -// Calculate timer interval, with all limits applied. -hal_timer_t Stepper::calc_timer_interval(uint32_t step_rate) { +#if HAS_STANDARD_MOTION - #ifdef CPU_32_BIT + // Calculate timer interval, with all limits applied. + hal_timer_t Stepper::calc_timer_interval(uint32_t step_rate) { - // A fast processor can just do integer division - return step_rate > minimal_step_rate ? uint32_t(STEPPER_TIMER_RATE) / step_rate : HAL_TIMER_TYPE_MAX; + #ifdef CPU_32_BIT - #else + // A fast processor can just do integer division + return step_rate > minimal_step_rate ? uint32_t(STEPPER_TIMER_RATE) / step_rate : HAL_TIMER_TYPE_MAX; - if (step_rate >= 0x0800) { // Higher step rate - // AVR is able to keep up at around 65kHz Stepping ISR rate at most. - // So values for step_rate > 65535 might as well be truncated. - // Handle it as quickly as possible. i.e., assume highest byte is zero - // because non-zero would represent a step rate far beyond AVR capabilities. - if (uint8_t(step_rate >> 16)) - return uint32_t(STEPPER_TIMER_RATE) / 0x10000; + #else - const uintptr_t table_address = uintptr_t(&speed_lookuptable_fast[uint8_t(step_rate >> 8)]); - const uint16_t base = uint16_t(pgm_read_word(table_address)); - const uint8_t gain = uint8_t(pgm_read_byte(table_address + 2)); - return base - MultiU8X8toH8(uint8_t(step_rate & 0x00FF), gain); - } - else if (step_rate > minimal_step_rate) { // Lower step rates - step_rate -= minimal_step_rate; // Correct for minimal speed - const uintptr_t table_address = uintptr_t(&speed_lookuptable_slow[uint8_t(step_rate >> 3)]); - return uint16_t(pgm_read_word(table_address)) - - ((uint16_t(pgm_read_word(table_address + 2)) * uint8_t(step_rate & 0x0007)) >> 3); - } + if (step_rate >= 0x0800) { // Higher step rate + // AVR is able to keep up at around 65kHz Stepping ISR rate at most. + // So values for step_rate > 65535 might as well be truncated. + // Handle it as quickly as possible. i.e., assume highest byte is zero + // because non-zero would represent a step rate far beyond AVR capabilities. + if (uint8_t(step_rate >> 16)) + return uint32_t(STEPPER_TIMER_RATE) / 0x10000; - return uint16_t(pgm_read_word(uintptr_t(speed_lookuptable_slow))); + const uintptr_t table_address = uintptr_t(&speed_lookuptable_fast[uint8_t(step_rate >> 8)]); + const uint16_t base = uint16_t(pgm_read_word(table_address)); + const uint8_t gain = uint8_t(pgm_read_byte(table_address + 2)); + return base - MultiU8X8toH8(uint8_t(step_rate & 0x00FF), gain); + } + else if (step_rate > minimal_step_rate) { // Lower step rates + step_rate -= minimal_step_rate; // Correct for minimal speed + const uintptr_t table_address = uintptr_t(&speed_lookuptable_slow[uint8_t(step_rate >> 3)]); + return uint16_t(pgm_read_word(table_address)) + - ((uint16_t(pgm_read_word(table_address + 2)) * uint8_t(step_rate & 0x0007)) >> 3); + } - #endif // !CPU_32_BIT -} + return uint16_t(pgm_read_word(uintptr_t(speed_lookuptable_slow))); -#if NONLINEAR_EXTRUSION_Q24 - void Stepper::calc_nonlinear_e(const uint32_t step_rate) { - const uint32_t velocity_q24 = ne.scale_q24 * step_rate; // Scale step_rate first so all intermediate values stay in range of 8.24 fixed point math - int32_t vd_q24 = ((((int64_t(ne.q24.A) * velocity_q24) >> 24) * velocity_q24) >> 24) + ((int64_t(ne.q24.B) * velocity_q24) >> 24); - NOLESS(vd_q24, 0); - - advance_dividend.e = (uint64_t(ne.q24.C + vd_q24) * ne.edividend) >> 24; + #endif // !CPU_32_BIT } -#endif -// Get the timer interval and the number of loops to perform per tick -hal_timer_t Stepper::calc_multistep_timer_interval(uint32_t step_rate) { + #if NONLINEAR_EXTRUSION_Q24 + void Stepper::calc_nonlinear_e(const uint32_t step_rate) { + const uint32_t velocity_q24 = ne.scale_q24 * step_rate; // Scale step_rate first so all intermediate values stay in range of 8.24 fixed point math + int32_t vd_q24 = ((((int64_t(ne.q24.A) * velocity_q24) >> 24) * velocity_q24) >> 24) + ((int64_t(ne.q24.B) * velocity_q24) >> 24); + NOLESS(vd_q24, 0); - #if ENABLED(OLD_ADAPTIVE_MULTISTEPPING) + advance_dividend.e = (uint64_t(ne.q24.C + vd_q24) * ne.edividend) >> 24; + } + #endif - #if MULTISTEPPING_LIMIT == 1 + // Get the timer interval and the number of loops to perform per tick + hal_timer_t Stepper::calc_multistep_timer_interval(uint32_t step_rate) { - // Just make sure the step rate is doable - NOMORE(step_rate, uint32_t(MAX_STEP_ISR_FREQUENCY_1X)); + #if ENABLED(OLD_ADAPTIVE_MULTISTEPPING) + #if MULTISTEPPING_LIMIT == 1 + + // Just make sure the step rate is doable + NOMORE(step_rate, uint32_t(MAX_STEP_ISR_FREQUENCY_1X)); + + #else + + // The stepping frequency limits for each multistepping rate + static const uint32_t limit[] PROGMEM = { + max_step_isr_frequency_sh(0) + , max_step_isr_frequency_sh(1) + #if MULTISTEPPING_LIMIT >= 4 + , max_step_isr_frequency_sh(2) + #endif + #if MULTISTEPPING_LIMIT >= 8 + , max_step_isr_frequency_sh(3) + #endif + #if MULTISTEPPING_LIMIT >= 16 + , max_step_isr_frequency_sh(4) + #endif + #if MULTISTEPPING_LIMIT >= 32 + , max_step_isr_frequency_sh(5) + #endif + #if MULTISTEPPING_LIMIT >= 64 + , max_step_isr_frequency_sh(6) + #endif + #if MULTISTEPPING_LIMIT >= 128 + , max_step_isr_frequency_sh(7) + #endif + }; + + // Find a doable step rate using multistepping + uint8_t multistep = 1; + for (uint8_t i = 0; i < COUNT(limit) && step_rate > uint32_t(pgm_read_dword(&limit[i])); ++i) { + step_rate >>= 1; + multistep <<= 1; + } + steps_per_isr = multistep; + + #endif + + #elif MULTISTEPPING_LIMIT > 1 + + uint8_t loops = steps_per_isr; + if (MULTISTEPPING_LIMIT >= 16 && loops >= 16) { step_rate >>= 4; loops >>= 4; } + if (MULTISTEPPING_LIMIT >= 4 && loops >= 4) { step_rate >>= 2; loops >>= 2; } + if (MULTISTEPPING_LIMIT >= 2 && loops >= 2) { step_rate >>= 1; } + + #endif + + return calc_timer_interval(step_rate); + } + + // Method to get all moving axes (for proper endstop handling) + void Stepper::set_axis_moved_for_current_block() { + + #if IS_CORE + // Define conditions for checking endstops + #define S_(N) current_block->steps[CORE_AXIS_##N] + #define D_(N) current_block->direction_bits[CORE_AXIS_##N] + #endif + + #if CORE_IS_XY || CORE_IS_XZ + /** + * Head direction in -X axis for CoreXY and CoreXZ bots. + * + * If steps differ, both axes are moving. + * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z, handled below) + * If DeltaA == DeltaB, the movement is only in the 1st axis (X) + */ + #if ANY(COREXY, COREXZ) + #define X_CMP(A,B) ((A)==(B)) + #else + #define X_CMP(A,B) ((A)!=(B)) + #endif + #define X_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && X_CMP(D_(1),D_(2))) ) + #elif ENABLED(MARKFORGED_XY) + #define X_MOVE_TEST (current_block->steps.a != current_block->steps.b) #else - - // The stepping frequency limits for each multistepping rate - static const uint32_t limit[] PROGMEM = { - max_step_isr_frequency_sh(0) - , max_step_isr_frequency_sh(1) - #if MULTISTEPPING_LIMIT >= 4 - , max_step_isr_frequency_sh(2) - #endif - #if MULTISTEPPING_LIMIT >= 8 - , max_step_isr_frequency_sh(3) - #endif - #if MULTISTEPPING_LIMIT >= 16 - , max_step_isr_frequency_sh(4) - #endif - #if MULTISTEPPING_LIMIT >= 32 - , max_step_isr_frequency_sh(5) - #endif - #if MULTISTEPPING_LIMIT >= 64 - , max_step_isr_frequency_sh(6) - #endif - #if MULTISTEPPING_LIMIT >= 128 - , max_step_isr_frequency_sh(7) - #endif - }; - - // Find a doable step rate using multistepping - uint8_t multistep = 1; - for (uint8_t i = 0; i < COUNT(limit) && step_rate > uint32_t(pgm_read_dword(&limit[i])); ++i) { - step_rate >>= 1; - multistep <<= 1; - } - steps_per_isr = multistep; - + #define X_MOVE_TEST !!current_block->steps.a #endif - #elif MULTISTEPPING_LIMIT > 1 - - uint8_t loops = steps_per_isr; - if (MULTISTEPPING_LIMIT >= 16 && loops >= 16) { step_rate >>= 4; loops >>= 4; } - if (MULTISTEPPING_LIMIT >= 4 && loops >= 4) { step_rate >>= 2; loops >>= 2; } - if (MULTISTEPPING_LIMIT >= 2 && loops >= 2) { step_rate >>= 1; } - - #endif - - return calc_timer_interval(step_rate); -} - -// Method to get all moving axes (for proper endstop handling) -void Stepper::set_axis_moved_for_current_block() { - - #if IS_CORE - // Define conditions for checking endstops - #define S_(N) current_block->steps[CORE_AXIS_##N] - #define D_(N) current_block->direction_bits[CORE_AXIS_##N] - #endif - - #if CORE_IS_XY || CORE_IS_XZ - /** - * Head direction in -X axis for CoreXY and CoreXZ bots. - * - * If steps differ, both axes are moving. - * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z, handled below) - * If DeltaA == DeltaB, the movement is only in the 1st axis (X) - */ - #if ANY(COREXY, COREXZ) - #define X_CMP(A,B) ((A)==(B)) + #if CORE_IS_XY || CORE_IS_YZ + /** + * Head direction in -Y axis for CoreXY / CoreYZ bots. + * + * If steps differ, both axes are moving + * If DeltaA == DeltaB, the movement is only in the 1st axis (X or Y) + * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z) + */ + #if ANY(COREYX, COREYZ) + #define Y_CMP(A,B) ((A)==(B)) + #else + #define Y_CMP(A,B) ((A)!=(B)) + #endif + #define Y_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && Y_CMP(D_(1),D_(2))) ) + #elif ENABLED(MARKFORGED_YX) + #define Y_MOVE_TEST (current_block->steps.a != current_block->steps.b) #else - #define X_CMP(A,B) ((A)!=(B)) + #define Y_MOVE_TEST !!current_block->steps.b #endif - #define X_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && X_CMP(D_(1),D_(2))) ) - #elif ENABLED(MARKFORGED_XY) - #define X_MOVE_TEST (current_block->steps.a != current_block->steps.b) - #else - #define X_MOVE_TEST !!current_block->steps.a - #endif - #if CORE_IS_XY || CORE_IS_YZ - /** - * Head direction in -Y axis for CoreXY / CoreYZ bots. - * - * If steps differ, both axes are moving - * If DeltaA == DeltaB, the movement is only in the 1st axis (X or Y) - * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z) - */ - #if ANY(COREYX, COREYZ) - #define Y_CMP(A,B) ((A)==(B)) + #if CORE_IS_XZ || CORE_IS_YZ + /** + * Head direction in -Z axis for CoreXZ or CoreYZ bots. + * + * If steps differ, both axes are moving + * If DeltaA == DeltaB, the movement is only in the 1st axis (X or Y, already handled above) + * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Z) + */ + #if ANY(COREZX, COREZY) + #define Z_CMP(A,B) ((A)==(B)) + #else + #define Z_CMP(A,B) ((A)!=(B)) + #endif + #define Z_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && Z_CMP(D_(1),D_(2))) ) #else - #define Y_CMP(A,B) ((A)!=(B)) + #define Z_MOVE_TEST !!current_block->steps.c #endif - #define Y_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && Y_CMP(D_(1),D_(2))) ) - #elif ENABLED(MARKFORGED_YX) - #define Y_MOVE_TEST (current_block->steps.a != current_block->steps.b) - #else - #define Y_MOVE_TEST !!current_block->steps.b - #endif - #if CORE_IS_XZ || CORE_IS_YZ - /** - * Head direction in -Z axis for CoreXZ or CoreYZ bots. - * - * If steps differ, both axes are moving - * If DeltaA == DeltaB, the movement is only in the 1st axis (X or Y, already handled above) - * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Z) - */ - #if ANY(COREZX, COREZY) - #define Z_CMP(A,B) ((A)==(B)) - #else - #define Z_CMP(A,B) ((A)!=(B)) - #endif - #define Z_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && Z_CMP(D_(1),D_(2))) ) - #else - #define Z_MOVE_TEST !!current_block->steps.c - #endif + // Set flags for all axes that move in this block + // These are set per-axis, not per-stepper + AxisBits didmove; + NUM_AXIS_CODE( + if (X_MOVE_TEST) didmove.a = true, // Cartesian X or Kinematic A + if (Y_MOVE_TEST) didmove.b = true, // Cartesian Y or Kinematic B + if (Z_MOVE_TEST) didmove.c = true, // Cartesian Z or Kinematic C + if (!!current_block->steps.i) didmove.i = true, + if (!!current_block->steps.j) didmove.j = true, + if (!!current_block->steps.k) didmove.k = true, + if (!!current_block->steps.u) didmove.u = true, + if (!!current_block->steps.v) didmove.v = true, + if (!!current_block->steps.w) didmove.w = true + ); + axis_did_move = didmove; + } - // Set flags for all axes that move in this block - // These are set per-axis, not per-stepper - AxisBits didmove; - NUM_AXIS_CODE( - if (X_MOVE_TEST) didmove.a = true, // Cartesian X or Kinematic A - if (Y_MOVE_TEST) didmove.b = true, // Cartesian Y or Kinematic B - if (Z_MOVE_TEST) didmove.c = true, // Cartesian Z or Kinematic C - if (!!current_block->steps.i) didmove.i = true, - if (!!current_block->steps.j) didmove.j = true, - if (!!current_block->steps.k) didmove.k = true, - if (!!current_block->steps.u) didmove.u = true, - if (!!current_block->steps.v) didmove.v = true, - if (!!current_block->steps.w) didmove.w = true - ); - axis_did_move = didmove; -} - -/** - * This last phase of the stepper interrupt processes and properly - * schedules planner blocks. This is executed after the step pulses - * have been done, so it is less time critical. - */ -hal_timer_t Stepper::block_phase_isr() { - #if DISABLED(OLD_ADAPTIVE_MULTISTEPPING) - // If the ISR uses < 50% of MPU time, halve multi-stepping - const hal_timer_t time_spent = HAL_timer_get_count(MF_TIMER_STEP); - #if MULTISTEPPING_LIMIT > 1 - if (steps_per_isr > 1 && time_spent_out_isr >= time_spent_in_isr + time_spent) { - steps_per_isr >>= 1; - // ticks_nominal will need to be recalculated if we are in cruise phase - ticks_nominal = 0; - } - #endif - time_spent_in_isr = -time_spent; // Unsigned but guaranteed to be +ve when needed - time_spent_out_isr = 0; - #endif - - // If no queued movements, just wait 1ms for the next block - hal_timer_t interval = (STEPPER_TIMER_RATE) / 1000UL; - - // If there is a current block - if (current_block) { - // If current block is finished, reset pointer and finalize state - if (step_events_completed >= step_event_count) { - #if ENABLED(DIRECT_STEPPING) - // Direct stepping is currently not ready for HAS_I_AXIS - #if STEPPER_PAGE_FORMAT == SP_4x4D_128 - #define PAGE_SEGMENT_UPDATE_POS(AXIS) \ - count_position[_AXIS(AXIS)] += page_step_state.bd[_AXIS(AXIS)] - 128 * 7; - #elif STEPPER_PAGE_FORMAT == SP_4x1_512 || STEPPER_PAGE_FORMAT == SP_4x2_256 - #define PAGE_SEGMENT_UPDATE_POS(AXIS) \ - count_position[_AXIS(AXIS)] += page_step_state.bd[_AXIS(AXIS)] * count_direction[_AXIS(AXIS)]; - #endif - - if (current_block->is_page()) { - PAGE_SEGMENT_UPDATE_POS(X); - PAGE_SEGMENT_UPDATE_POS(Y); - PAGE_SEGMENT_UPDATE_POS(Z); - PAGE_SEGMENT_UPDATE_POS(E); + /** + * This last phase of the stepper interrupt processes and properly + * schedules planner blocks. This is executed after the step pulses + * have been done, so it is less time critical. + */ + hal_timer_t Stepper::block_phase_isr() { + #if DISABLED(OLD_ADAPTIVE_MULTISTEPPING) + // If the ISR uses < 50% of MPU time, halve multi-stepping + const hal_timer_t time_spent = HAL_timer_get_count(MF_TIMER_STEP); + #if MULTISTEPPING_LIMIT > 1 + if (steps_per_isr > 1 && time_spent_out_isr >= time_spent_in_isr + time_spent) { + steps_per_isr >>= 1; + // ticks_nominal will need to be recalculated if we are in cruise phase + ticks_nominal = 0; } #endif - TERN_(HAS_FILAMENT_RUNOUT_DISTANCE, runout.block_completed(current_block)); - discard_current_block(); - } - else { - // Step events not completed yet... + time_spent_in_isr = -time_spent; // Unsigned but guaranteed to be +ve when needed + time_spent_out_isr = 0; + #endif - // Are we in acceleration phase ? - if (step_events_completed < accelerate_before) { // Calculate new timer value + // If no queued movements, just wait 1ms for the next block + hal_timer_t interval = (STEPPER_TIMER_RATE) / 1000UL; - #if ENABLED(S_CURVE_ACCELERATION) - // Get the next speed to use (Jerk limited!) - uint32_t acc_step_rate = acceleration_time < current_block->acceleration_time - ? _eval_bezier_curve(acceleration_time) - : current_block->cruise_rate; - #else - acc_step_rate = STEP_MULTIPLY(acceleration_time, current_block->acceleration_rate) + current_block->initial_rate; - NOMORE(acc_step_rate, current_block->nominal_rate); - #endif - - // acc_step_rate is in steps/second - - // step_rate to timer interval and steps per stepper isr - interval = calc_multistep_timer_interval(acc_step_rate << oversampling_factor); - acceleration_time += interval; - deceleration_time = 0; // Reset since we're doing acceleration first. - - // Apply Nonlinear Extrusion, if enabled - calc_nonlinear_e(acc_step_rate << oversampling_factor); - - #if HAS_ROUGH_LIN_ADVANCE - if (la_active) { - const uint32_t la_step_rate = la_advance_steps < current_block->max_adv_steps ? current_block->la_advance_rate : 0; - la_interval = calc_timer_interval((acc_step_rate + la_step_rate) >> current_block->la_scaling); - } - #endif - - /** - * Adjust Laser Power - Accelerating - * - * isPowered - True when a move is powered. - * isEnabled - laser power is active. - * - * Laser power variables are calculated and stored in this block by the planner code. - * trap_ramp_active_pwr - the active power in this block across accel or decel trap steps. - * trap_ramp_entry_incr - holds the precalculated value to increase the current power per accel step. - */ - #if ENABLED(LASER_POWER_TRAP) - if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { - if (planner.laser_inline.status.isPowered && planner.laser_inline.status.isEnabled) { - if (current_block->laser.trap_ramp_entry_incr > 0) { - cutter.apply_power(current_block->laser.trap_ramp_active_pwr); - current_block->laser.trap_ramp_active_pwr += current_block->laser.trap_ramp_entry_incr * steps_per_isr; - } - } - // Not a powered move. - else cutter.apply_power(0); - } - #endif - TERN_(SMOOTH_LIN_ADVANCE, curr_step_rate = acc_step_rate); - } - // Are we in Deceleration phase ? - else if (step_events_completed >= decelerate_start) { - uint32_t step_rate; - - #if ENABLED(S_CURVE_ACCELERATION) - // If this is the 1st time we process the 2nd half of the trapezoid... - if (!bezier_2nd_half) { - // Initialize the Bézier speed curve - _calc_bezier_curve_coeffs(current_block->cruise_rate, current_block->final_rate, current_block->deceleration_time_inverse); - bezier_2nd_half = true; - } - // Calculate the next speed to use - step_rate = deceleration_time < current_block->deceleration_time - ? _eval_bezier_curve(deceleration_time) - : current_block->final_rate; - #else - // Using the old trapezoidal control - step_rate = STEP_MULTIPLY(deceleration_time, current_block->acceleration_rate); - if (step_rate < acc_step_rate) { - step_rate = acc_step_rate - step_rate; - NOLESS(step_rate, current_block->final_rate); - } - else - step_rate = current_block->final_rate; - - #endif - - // step_rate to timer interval and steps per stepper isr - interval = calc_multistep_timer_interval(step_rate << oversampling_factor); - deceleration_time += interval; - - // Apply Nonlinear Extrusion, if enabled - calc_nonlinear_e(step_rate << oversampling_factor); - - #if HAS_ROUGH_LIN_ADVANCE - if (la_active) { - const uint32_t la_step_rate = la_advance_steps > current_block->final_adv_steps ? current_block->la_advance_rate : 0; - if (la_step_rate != step_rate) { - const bool forward_e = la_step_rate < step_rate; - la_interval = calc_timer_interval((forward_e ? step_rate - la_step_rate : la_step_rate - step_rate) >> current_block->la_scaling); - - if (forward_e != motor_direction(E_AXIS)) { - last_direction_bits.toggle(E_AXIS); - count_direction.e *= -1; - - DIR_WAIT_BEFORE(); - - E_APPLY_DIR(forward_e, false); - - TERN_(FT_MOTION, last_set_direction = last_direction_bits); - - DIR_WAIT_AFTER(); - } - } - else - la_interval = LA_ADV_NEVER; - } - #endif // LIN_ADVANCE - - // Adjust Laser Power - Decelerating - #if ENABLED(LASER_POWER_TRAP) - if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { - if (planner.laser_inline.status.isPowered && planner.laser_inline.status.isEnabled) { - if (current_block->laser.trap_ramp_exit_decr > 0) { - current_block->laser.trap_ramp_active_pwr -= current_block->laser.trap_ramp_exit_decr * steps_per_isr; - cutter.apply_power(current_block->laser.trap_ramp_active_pwr); - } - // Not a powered move. - else cutter.apply_power(0); - } - } - #endif - TERN_(SMOOTH_LIN_ADVANCE, curr_step_rate = step_rate); - } - else { // Must be in cruise phase otherwise - - // Calculate the ticks_nominal for this nominal speed, if not done yet - if (ticks_nominal == 0) { - // step_rate to timer interval and loops for the nominal speed - ticks_nominal = calc_multistep_timer_interval(current_block->nominal_rate << oversampling_factor); - // Prepare for deceleration - IF_DISABLED(S_CURVE_ACCELERATION, acc_step_rate = current_block->nominal_rate); - TERN_(SMOOTH_LIN_ADVANCE, curr_step_rate = current_block->nominal_rate); - deceleration_time = ticks_nominal / 2; - - // Apply Nonlinear Extrusion, if enabled - calc_nonlinear_e(current_block->nominal_rate << oversampling_factor); - - #if HAS_ROUGH_LIN_ADVANCE - if (la_active) - la_interval = calc_timer_interval(current_block->nominal_rate >> current_block->la_scaling); + // If there is a current block + if (current_block) { + // If current block is finished, reset pointer and finalize state + if (step_events_completed >= step_event_count) { + #if ENABLED(DIRECT_STEPPING) + // Direct stepping is currently not ready for HAS_I_AXIS + #if STEPPER_PAGE_FORMAT == SP_4x4D_128 + #define PAGE_SEGMENT_UPDATE_POS(AXIS) \ + count_position[_AXIS(AXIS)] += page_step_state.bd[_AXIS(AXIS)] - 128 * 7; + #elif STEPPER_PAGE_FORMAT == SP_4x1_512 || STEPPER_PAGE_FORMAT == SP_4x2_256 + #define PAGE_SEGMENT_UPDATE_POS(AXIS) \ + count_position[_AXIS(AXIS)] += page_step_state.bd[_AXIS(AXIS)] * count_direction[_AXIS(AXIS)]; #endif - // Adjust Laser Power - Cruise + if (current_block->is_page()) { + PAGE_SEGMENT_UPDATE_POS(X); + PAGE_SEGMENT_UPDATE_POS(Y); + PAGE_SEGMENT_UPDATE_POS(Z); + PAGE_SEGMENT_UPDATE_POS(E); + } + #endif + TERN_(HAS_FILAMENT_RUNOUT_DISTANCE, runout.block_completed(current_block)); + discard_current_block(); + } + else { + // Step events not completed yet... + + // Are we in acceleration phase ? + if (step_events_completed < accelerate_before) { // Calculate new timer value + + #if ENABLED(S_CURVE_ACCELERATION) + // Get the next speed to use (Jerk limited!) + uint32_t acc_step_rate = acceleration_time < current_block->acceleration_time + ? _eval_bezier_curve(acceleration_time) + : current_block->cruise_rate; + #else + acc_step_rate = STEP_MULTIPLY(acceleration_time, current_block->acceleration_rate) + current_block->initial_rate; + NOMORE(acc_step_rate, current_block->nominal_rate); + #endif + + // acc_step_rate is in steps/second + + // step_rate to timer interval and steps per stepper isr + interval = calc_multistep_timer_interval(acc_step_rate << oversampling_factor); + acceleration_time += interval; + deceleration_time = 0; // Reset since we're doing acceleration first. + + // Apply Nonlinear Extrusion, if enabled + calc_nonlinear_e(acc_step_rate << oversampling_factor); + + #if HAS_ROUGH_LIN_ADVANCE + if (la_active) { + const uint32_t la_step_rate = la_advance_steps < current_block->max_adv_steps ? current_block->la_advance_rate : 0; + la_interval = calc_timer_interval((acc_step_rate + la_step_rate) >> current_block->la_scaling); + } + #endif + + /** + * Adjust Laser Power - Accelerating + * + * isPowered - True when a move is powered. + * isEnabled - laser power is active. + * + * Laser power variables are calculated and stored in this block by the planner code. + * trap_ramp_active_pwr - the active power in this block across accel or decel trap steps. + * trap_ramp_entry_incr - holds the precalculated value to increase the current power per accel step. + */ #if ENABLED(LASER_POWER_TRAP) if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { if (planner.laser_inline.status.isPowered && planner.laser_inline.status.isEnabled) { if (current_block->laser.trap_ramp_entry_incr > 0) { - current_block->laser.trap_ramp_active_pwr = current_block->laser.power; - cutter.apply_power(current_block->laser.power); + cutter.apply_power(current_block->laser.trap_ramp_active_pwr); + current_block->laser.trap_ramp_active_pwr += current_block->laser.trap_ramp_entry_incr * steps_per_isr; } } // Not a powered move. else cutter.apply_power(0); } #endif + TERN_(SMOOTH_LIN_ADVANCE, curr_step_rate = acc_step_rate); } + // Are we in Deceleration phase ? + else if (step_events_completed >= decelerate_start) { + uint32_t step_rate; - // The timer interval is just the nominal value for the nominal speed - interval = ticks_nominal; + #if ENABLED(S_CURVE_ACCELERATION) + // If this is the 1st time we process the 2nd half of the trapezoid... + if (!bezier_2nd_half) { + // Initialize the Bézier speed curve + _calc_bezier_curve_coeffs(current_block->cruise_rate, current_block->final_rate, current_block->deceleration_time_inverse); + bezier_2nd_half = true; + } + // Calculate the next speed to use + step_rate = deceleration_time < current_block->deceleration_time + ? _eval_bezier_curve(deceleration_time) + : current_block->final_rate; + #else + // Using the old trapezoidal control + step_rate = STEP_MULTIPLY(deceleration_time, current_block->acceleration_rate); + if (step_rate < acc_step_rate) { + step_rate = acc_step_rate - step_rate; + NOLESS(step_rate, current_block->final_rate); + } + else + step_rate = current_block->final_rate; + + #endif + + // step_rate to timer interval and steps per stepper isr + interval = calc_multistep_timer_interval(step_rate << oversampling_factor); + deceleration_time += interval; + + // Apply Nonlinear Extrusion, if enabled + calc_nonlinear_e(step_rate << oversampling_factor); + + #if HAS_ROUGH_LIN_ADVANCE + if (la_active) { + const uint32_t la_step_rate = la_advance_steps > current_block->final_adv_steps ? current_block->la_advance_rate : 0; + if (la_step_rate != step_rate) { + const bool forward_e = la_step_rate < step_rate; + la_interval = calc_timer_interval((forward_e ? step_rate - la_step_rate : la_step_rate - step_rate) >> current_block->la_scaling); + + if (forward_e != motor_direction(E_AXIS)) { + last_direction_bits.toggle(E_AXIS); + count_direction.e *= -1; + + DIR_WAIT_BEFORE(); + + E_APPLY_DIR(forward_e, false); + + TERN_(FT_MOTION, last_set_direction = last_direction_bits); + + DIR_WAIT_AFTER(); + } + } + else + la_interval = LA_ADV_NEVER; + } + #endif // LIN_ADVANCE + + // Adjust Laser Power - Decelerating + #if ENABLED(LASER_POWER_TRAP) + if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { + if (planner.laser_inline.status.isPowered && planner.laser_inline.status.isEnabled) { + if (current_block->laser.trap_ramp_exit_decr > 0) { + current_block->laser.trap_ramp_active_pwr -= current_block->laser.trap_ramp_exit_decr * steps_per_isr; + cutter.apply_power(current_block->laser.trap_ramp_active_pwr); + } + // Not a powered move. + else cutter.apply_power(0); + } + } + #endif + TERN_(SMOOTH_LIN_ADVANCE, curr_step_rate = step_rate); + } + else { // Must be in cruise phase otherwise + + // Calculate the ticks_nominal for this nominal speed, if not done yet + if (ticks_nominal == 0) { + // step_rate to timer interval and loops for the nominal speed + ticks_nominal = calc_multistep_timer_interval(current_block->nominal_rate << oversampling_factor); + deceleration_time = ticks_nominal / 2; + + // Prepare for deceleration + IF_DISABLED(S_CURVE_ACCELERATION, acc_step_rate = current_block->nominal_rate); + TERN_(SMOOTH_LIN_ADVANCE, curr_step_rate = current_block->nominal_rate); + + // Apply Nonlinear Extrusion, if enabled + calc_nonlinear_e(current_block->nominal_rate << oversampling_factor); + + #if HAS_ROUGH_LIN_ADVANCE + if (la_active) + la_interval = calc_timer_interval(current_block->nominal_rate >> current_block->la_scaling); + #endif + + // Adjust Laser Power - Cruise + #if ENABLED(LASER_POWER_TRAP) + if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { + if (planner.laser_inline.status.isPowered && planner.laser_inline.status.isEnabled) { + if (current_block->laser.trap_ramp_entry_incr > 0) { + current_block->laser.trap_ramp_active_pwr = current_block->laser.power; + cutter.apply_power(current_block->laser.power); + } + } + // Not a powered move. + else cutter.apply_power(0); + } + #endif + } + + // The timer interval is just the nominal value for the nominal speed + interval = ticks_nominal; + } } + + #if ENABLED(LASER_FEATURE) + /** + * CUTTER_MODE_DYNAMIC is experimental and developing. + * Super-fast method to dynamically adjust the laser power OCR value based on the input feedrate in mm-per-minute. + * TODO: Set up Min/Max OCR offsets to allow tuning and scaling of various lasers. + * TODO: Integrate accel/decel +-rate into the dynamic laser power calc. + */ + if (cutter.cutter_mode == CUTTER_MODE_DYNAMIC + && planner.laser_inline.status.isPowered // isPowered flag set on any parsed G1, G2, G3, or G5 move; cleared on any others. + && current_block // Block may not be available if steps completed (see discard_current_block() above) + && cutter.last_block_power != current_block->laser.power // Prevent constant update without change + ) { + cutter.apply_power(current_block->laser.power); + cutter.last_block_power = current_block->laser.power; + } + #endif + } + else { // !current_block + #if ENABLED(LASER_FEATURE) + if (cutter.cutter_mode == CUTTER_MODE_DYNAMIC) + cutter.apply_power(0); // No movement in dynamic mode so turn Laser off + #endif } - #if ENABLED(LASER_FEATURE) - /** - * CUTTER_MODE_DYNAMIC is experimental and developing. - * Super-fast method to dynamically adjust the laser power OCR value based on the input feedrate in mm-per-minute. - * TODO: Set up Min/Max OCR offsets to allow tuning and scaling of various lasers. - * TODO: Integrate accel/decel +-rate into the dynamic laser power calc. - */ - if (cutter.cutter_mode == CUTTER_MODE_DYNAMIC - && planner.laser_inline.status.isPowered // isPowered flag set on any parsed G1, G2, G3, or G5 move; cleared on any others. - && current_block // Block may not be available if steps completed (see discard_current_block() above) - && cutter.last_block_power != current_block->laser.power // Prevent constant update without change - ) { - cutter.apply_power(current_block->laser.power); - cutter.last_block_power = current_block->laser.power; - } - #endif - } - else { // !current_block - #if ENABLED(LASER_FEATURE) - if (cutter.cutter_mode == CUTTER_MODE_DYNAMIC) - cutter.apply_power(0); // No movement in dynamic mode so turn Laser off - #endif - } + // If there is no current block at this point, attempt to pop one from the buffer + // and prepare its movement + if (!current_block) { - // If there is no current block at this point, attempt to pop one from the buffer - // and prepare its movement - if (!current_block) { + // Anything in the buffer? + if ((current_block = planner.get_current_block())) { - // Anything in the buffer? - if ((current_block = planner.get_current_block())) { + // Run through all sync blocks + while (current_block->is_sync()) { - // Run through all sync blocks - while (current_block->is_sync()) { + // Set laser power + #if ENABLED(LASER_POWER_SYNC) + if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { + if (current_block->is_sync_pwr()) { + planner.laser_inline.status.isSyncPower = true; + cutter.apply_power(current_block->laser.power); + } + } + #endif - // Set laser power - #if ENABLED(LASER_POWER_SYNC) - if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { - if (current_block->is_sync_pwr()) { - planner.laser_inline.status.isSyncPower = true; - cutter.apply_power(current_block->laser.power); + // Set "fan speeds" for a laser module + #if ENABLED(LASER_SYNCHRONOUS_M106_M107) + if (current_block->is_sync_fan()) planner.sync_fan_speeds(current_block->fan_speed); + #endif + + // Set position + if (current_block->is_sync_pos()) _set_position(current_block->position); + + // Done with this block + discard_current_block(); + + // Try to get a new block. Exit if there are no more. + if (!(current_block = planner.get_current_block())) + return interval; // No more queued movements! + } + + // For non-inline cutter, grossly apply power + #if HAS_CUTTER + if (cutter.cutter_mode == CUTTER_MODE_STANDARD) { + cutter.apply_power(current_block->cutter_power); + } + #endif + + #if ENABLED(POWER_LOSS_RECOVERY) + recovery.info.sdpos = current_block->sdpos; + recovery.info.current_position = current_block->start_position; + #endif + + #if ENABLED(DIRECT_STEPPING) + if (current_block->is_page()) { + page_step_state.segment_steps = 0; + page_step_state.segment_idx = 0; + page_step_state.page = page_manager.get_page(current_block->page_idx); + page_step_state.bd.reset(); + + if (DirectStepping::Config::DIRECTIONAL) + current_block->direction_bits = last_direction_bits; + + if (!page_step_state.page) { + discard_current_block(); + return interval; } } #endif - // Set "fan speeds" for a laser module - #if ENABLED(LASER_SYNCHRONOUS_M106_M107) - if (current_block->is_sync_fan()) planner.sync_fan_speeds(current_block->fan_speed); + // Set flags for all moving axes, accounting for kinematics + set_axis_moved_for_current_block(); + + #if ENABLED(ADAPTIVE_STEP_SMOOTHING) + oversampling_factor = 0; + + // Decide if axis smoothing is possible + if (adaptive_step_smoothing_enabled) { + uint32_t max_rate = current_block->nominal_rate; // Get the step event rate + while (max_rate < min_step_isr_frequency) { // As long as more ISRs are possible... + max_rate <<= 1; // Try to double the rate + if (max_rate < min_step_isr_frequency) // Don't exceed the estimated ISR limit + ++oversampling_factor; // Increase the oversampling (used for left-shift) + } + } #endif - // Set position - if (current_block->is_sync_pos()) _set_position(current_block->position); + // Based on the oversampling factor, do the calculations + step_event_count = current_block->step_event_count << oversampling_factor; - // Done with this block - discard_current_block(); + #if ENABLED(DIFFERENTIAL_EXTRUDER) - // Try to get a new block. Exit if there are no more. - if (!(current_block = planner.get_current_block())) - return interval; // No more queued movements! - } + // X and E work together as a differential mechanism: + // When X and E move in the same direction they move the print carriage, + // and when X and E move in opposite directions they cause extrusion. - // For non-inline cutter, grossly apply power - #if HAS_CUTTER - if (cutter.cutter_mode == CUTTER_MODE_STANDARD) { - cutter.apply_power(current_block->cutter_power); - } - #endif + // NOTE: All calculations performed per block, preserving Bresenham timing coordination + if (current_block->steps.x > 0 || current_block->steps.e > 0) { + // Calculate signed step counts based on directions + const int32_t x_signed_steps = current_block->direction_bits.x ? current_block->steps.x : -int32_t(current_block->steps.x), + e_signed_steps = current_block->direction_bits.e ? current_block->steps.e : -int32_t(current_block->steps.e); - #if ENABLED(POWER_LOSS_RECOVERY) - recovery.info.sdpos = current_block->sdpos; - recovery.info.current_position = current_block->start_position; - #endif + #if ENABLED(BALANCED_DIFFERENTIAL_EXTRUDER) - #if ENABLED(DIRECT_STEPPING) - if (current_block->is_page()) { - page_step_state.segment_steps = 0; - page_step_state.segment_idx = 0; - page_step_state.page = page_manager.get_page(current_block->page_idx); - page_step_state.bd.reset(); + // BALANCED DIFFERENTIAL EXTRUDER: X stepper drives one loop belt + // and E stepper drives another loop belt. Two belts mesh at the extruder + // X stepper: X - E/2 (carriage movement - half extrusion) + // E stepper: X + E/2 (carriage movement + half extrusion) + // Net extrusion: (X + E/2) - (X - E/2) = E + // (This will work great once I figure out what to do when E/2 is not integer!) - if (DirectStepping::Config::DIRECTIONAL) - current_block->direction_bits = last_direction_bits; + // Split extrusion 50/50 between X and E steppers + const int32_t half_e_steps = e_signed_steps / 2; + + // X stepper: X movement - half E extrusion + const int32_t new_x_steps = x_signed_steps - half_e_steps; + current_block->steps.x = ABS(new_x_steps); + + // Update X axis direction + current_block->direction_bits.x = new_x_steps >= 0; + + // E stepper: X movement + half E extrusion + const int32_t new_e_steps = x_signed_steps + half_e_steps; + current_block->steps.e = ABS(new_e_steps); + + // Update E axis direction + current_block->direction_bits.e = new_e_steps >= 0; + + #else // !BALANCED_DIFFERENTIAL_EXTRUDER + + // SIMPLE SINGLE-LOOP DIFFERENTIAL EXTRUDER: X stepper drives the carriage as usual, + // while E stepper drives a loop belt that's meshed around the extruder. + // X stepper: X only (carriage movement) + // E stepper: X + E (carriage movement + extrusion) + // Net extrusion: (X + E) - X = E + + // Calculate net E steps (X movement + extrusion) + const int32_t net_e_steps = x_signed_steps + e_signed_steps; + + // Update the block with new E step count + current_block->steps.e = ABS(net_e_steps); + + // Update direction bit for E axis + current_block->direction_bits.e = net_e_steps >= 0; + + #endif // !BALANCED_DIFFERENTIAL_EXTRUDER + + // NOTE: DO NOT modify the step_event_count! This would change XYZ timing! + // Bresenham distributes X and E steps over the time base. - if (!page_step_state.page) { - discard_current_block(); - return interval; } - } - #endif - // Set flags for all moving axes, accounting for kinematics - set_axis_moved_for_current_block(); + #endif // DIFFERENTIAL_EXTRUDER - #if ENABLED(ADAPTIVE_STEP_SMOOTHING) - oversampling_factor = 0; + // Initialize Bresenham delta errors to 1/2 + delta_error = -int32_t(step_event_count); + TERN_(HAS_ROUGH_LIN_ADVANCE, la_delta_error = delta_error); - // Decide if axis smoothing is possible - if (adaptive_step_smoothing_enabled) { - uint32_t max_rate = current_block->nominal_rate; // Get the step event rate - while (max_rate < min_step_isr_frequency) { // As long as more ISRs are possible... - max_rate <<= 1; // Try to double the rate - if (max_rate < min_step_isr_frequency) // Don't exceed the estimated ISR limit - ++oversampling_factor; // Increase the oversampling (used for left-shift) + // Calculate Bresenham dividends and divisors + advance_dividend = (current_block->steps << 1).asInt32(); + advance_divisor = step_event_count << 1; + + #if ENABLED(INPUT_SHAPING_X) + if (shaping_x.enabled) { + const int64_t steps = current_block->direction_bits.x ? int64_t(current_block->steps.x) : -int64_t(current_block->steps.x); + shaping_x.last_block_end_pos += steps; + + // If there are any remaining echos unprocessed, then direction change must + // be delayed and processed in PULSE_PREP_SHAPING. This will cause half a step + // to be missed, which will need recovering and this can be done through shaping_x.remainder. + shaping_x.forward = current_block->direction_bits.x; + if (!ShapingQueue::empty_x()) current_block->direction_bits.x = last_direction_bits.x; } - } - #endif - - // Based on the oversampling factor, do the calculations - step_event_count = current_block->step_event_count << oversampling_factor; - - #if ENABLED(DIFFERENTIAL_EXTRUDER) - - // X and E work together as a differential mechanism: - // When X and E move in the same direction they move the print carriage, - // and when X and E move in opposite directions they cause extrusion. - - // NOTE: All calculations performed per block, preserving Bresenham timing coordination - if (current_block->steps.x > 0 || current_block->steps.e > 0) { - // Calculate signed step counts based on directions - const int32_t x_signed_steps = current_block->direction_bits.x ? current_block->steps.x : -int32_t(current_block->steps.x), - e_signed_steps = current_block->direction_bits.e ? current_block->steps.e : -int32_t(current_block->steps.e); - - #if ENABLED(BALANCED_DIFFERENTIAL_EXTRUDER) - - // BALANCED DIFFERENTIAL EXTRUDER: X stepper drives one loop belt - // and E stepper drives another loop belt. Two belts mesh at the extruder - // X stepper: X - E/2 (carriage movement - half extrusion) - // E stepper: X + E/2 (carriage movement + half extrusion) - // Net extrusion: (X + E/2) - (X - E/2) = E - // (This will work great once I figure out what to do when E/2 is not integer!) - - // Split extrusion 50/50 between X and E steppers - const int32_t half_e_steps = e_signed_steps / 2; - - // X stepper: X movement - half E extrusion - const int32_t new_x_steps = x_signed_steps - half_e_steps; - current_block->steps.x = ABS(new_x_steps); - - // Update X axis direction - current_block->direction_bits.x = new_x_steps >= 0; - - // E stepper: X movement + half E extrusion - const int32_t new_e_steps = x_signed_steps + half_e_steps; - current_block->steps.e = ABS(new_e_steps); - - // Update E axis direction - current_block->direction_bits.e = new_e_steps >= 0; - - #else // !BALANCED_DIFFERENTIAL_EXTRUDER - - // SIMPLE SINGLE-LOOP DIFFERENTIAL EXTRUDER: X stepper drives the carriage as usual, - // while E stepper drives a loop belt that's meshed around the extruder. - // X stepper: X only (carriage movement) - // E stepper: X + E (carriage movement + extrusion) - // Net extrusion: (X + E) - X = E - - // Calculate net E steps (X movement + extrusion) - const int32_t net_e_steps = x_signed_steps + e_signed_steps; - - // Update the block with new E step count - current_block->steps.e = ABS(net_e_steps); - - // Update direction bit for E axis - current_block->direction_bits.e = net_e_steps >= 0; - - #endif // !BALANCED_DIFFERENTIAL_EXTRUDER - - // NOTE: DO NOT modify the step_event_count! This would change XYZ timing! - // Bresenham distributes X and E steps over the time base. - - } - - #endif // DIFFERENTIAL_EXTRUDER - - // Initialize Bresenham delta errors to 1/2 - delta_error = -int32_t(step_event_count); - TERN_(HAS_ROUGH_LIN_ADVANCE, la_delta_error = delta_error); - - // Calculate Bresenham dividends and divisors - advance_dividend = (current_block->steps << 1).asInt32(); - advance_divisor = step_event_count << 1; - - #if ENABLED(INPUT_SHAPING_X) - if (shaping_x.enabled) { - const int64_t steps = current_block->direction_bits.x ? int64_t(current_block->steps.x) : -int64_t(current_block->steps.x); - shaping_x.last_block_end_pos += steps; - - // If there are any remaining echos unprocessed, then direction change must - // be delayed and processed in PULSE_PREP_SHAPING. This will cause half a step - // to be missed, which will need recovering and this can be done through shaping_x.remainder. - shaping_x.forward = current_block->direction_bits.x; - if (!ShapingQueue::empty_x()) current_block->direction_bits.x = last_direction_bits.x; - } - #endif - - // Y and Z follow the same logic as X (but the comments aren't repeated) - #if ENABLED(INPUT_SHAPING_Y) - if (shaping_y.enabled) { - const int64_t steps = current_block->direction_bits.y ? int64_t(current_block->steps.y) : -int64_t(current_block->steps.y); - shaping_y.last_block_end_pos += steps; - shaping_y.forward = current_block->direction_bits.y; - if (!ShapingQueue::empty_y()) current_block->direction_bits.y = last_direction_bits.y; - } - #endif - - #if ENABLED(INPUT_SHAPING_Z) - if (shaping_z.enabled) { - const int64_t steps = current_block->direction_bits.z ? int64_t(current_block->steps.z) : -int64_t(current_block->steps.z); - shaping_z.last_block_end_pos += steps; - shaping_z.forward = current_block->direction_bits.z; - if (!ShapingQueue::empty_z()) current_block->direction_bits.z = last_direction_bits.z; - } - #endif - - // No step events completed so far - step_events_completed = 0; - - // Compute the acceleration and deceleration points - accelerate_before = current_block->accelerate_before << oversampling_factor; - decelerate_start = current_block->decelerate_start << oversampling_factor; - - TERN_(MIXING_EXTRUDER, mixer.stepper_setup(current_block->b_color)); - - E_TERN_(stepper_extruder = current_block->extruder); - - // Initialize the trapezoid generator from the current block. - #if HAS_ROUGH_LIN_ADVANCE - #if DISABLED(MIXING_EXTRUDER) && E_STEPPERS > 1 - // If the now active extruder wasn't in use during the last move, its pressure is most likely gone. - if (stepper_extruder != last_moved_extruder) la_advance_steps = 0; #endif - la_active = (current_block->la_advance_rate != 0); - if (la_active) { - // Apply LA scaling and discount the effect of frequency scaling - la_dividend = (advance_dividend.e << current_block->la_scaling) << oversampling_factor; - } - #endif - if ( ENABLED(DUAL_X_CARRIAGE) // TODO: Find out why this fixes "jittery" small circles - || current_block->direction_bits != last_direction_bits - || TERN(MIXING_EXTRUDER, false, stepper_extruder != last_moved_extruder) - ) { - E_TERN_(last_moved_extruder = stepper_extruder); - set_directions(current_block->direction_bits); - } - - #if ENABLED(LASER_FEATURE) - if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { // Planner controls the laser - if (planner.laser_inline.status.isSyncPower) - // If the previous block was a M3 sync power then skip the trap power init otherwise it will 0 the sync power. - planner.laser_inline.status.isSyncPower = false; // Clear the flag to process subsequent trap calc's. - else if (current_block->laser.status.isEnabled) { - #if ENABLED(LASER_POWER_TRAP) - TERN_(DEBUG_LASER_TRAP, SERIAL_ECHO_MSG("InitTrapPwr:", current_block->laser.trap_ramp_active_pwr)); - cutter.apply_power(current_block->laser.status.isPowered ? current_block->laser.trap_ramp_active_pwr : 0); - #else - TERN_(DEBUG_CUTTER_POWER, SERIAL_ECHO_MSG("InlinePwr:", current_block->laser.power)); - cutter.apply_power(current_block->laser.status.isPowered ? current_block->laser.power : 0); - #endif + // Y and Z follow the same logic as X (but the comments aren't repeated) + #if ENABLED(INPUT_SHAPING_Y) + if (shaping_y.enabled) { + const int64_t steps = current_block->direction_bits.y ? int64_t(current_block->steps.y) : -int64_t(current_block->steps.y); + shaping_y.last_block_end_pos += steps; + shaping_y.forward = current_block->direction_bits.y; + if (!ShapingQueue::empty_y()) current_block->direction_bits.y = last_direction_bits.y; } - } - #endif // LASER_FEATURE + #endif - // If the endstop is already pressed, endstop interrupts won't invoke - // endstop_triggered and the move will grind. So check here for a - // triggered endstop, which marks the block for discard on the next ISR. - endstops.update(); + #if ENABLED(INPUT_SHAPING_Z) + if (shaping_z.enabled) { + const int64_t steps = current_block->direction_bits.z ? int64_t(current_block->steps.z) : -int64_t(current_block->steps.z); + shaping_z.last_block_end_pos += steps; + shaping_z.forward = current_block->direction_bits.z; + if (!ShapingQueue::empty_z()) current_block->direction_bits.z = last_direction_bits.z; + } + #endif - #if ENABLED(Z_LATE_ENABLE) - // If delayed Z enable, enable it now. This option will severely interfere with - // timing between pulses when chaining motion between blocks, and it could lead - // to lost steps in both X and Y axis, so avoid using it unless strictly necessary!! - if (current_block->steps.z) enable_axis(Z_AXIS); - #endif + // No step events completed so far + step_events_completed = 0; - // Mark ticks_nominal as not-yet-calculated - ticks_nominal = 0; + // Compute the acceleration and deceleration points + accelerate_before = current_block->accelerate_before << oversampling_factor; + decelerate_start = current_block->decelerate_start << oversampling_factor; - #if ENABLED(S_CURVE_ACCELERATION) - // Initialize the Bézier speed curve - _calc_bezier_curve_coeffs(current_block->initial_rate, current_block->cruise_rate, current_block->acceleration_time_inverse); - // We haven't started the 2nd half of the trapezoid - bezier_2nd_half = false; - #else - // Set as deceleration point the initial rate of the block - acc_step_rate = current_block->initial_rate; - #endif + TERN_(MIXING_EXTRUDER, mixer.stepper_setup(current_block->b_color)); - // Calculate Nonlinear Extrusion fixed-point quotients - #if NONLINEAR_EXTRUSION_Q24 - ne.edividend = advance_dividend.e; - const float scale = (float(ne.edividend) / advance_divisor) * planner.mm_per_step[E_AXIS_N(current_block->extruder)]; - ne.scale_q24 = _BV32(24) * scale; - if (ne.settings.enabled && current_block->direction_bits.e && ANY_AXIS_MOVES(current_block)) { - ne.q24.A = _BV32(24) * ne.settings.coeff.A; - ne.q24.B = _BV32(24) * ne.settings.coeff.B; - ne.q24.C = _BV32(24) * ne.settings.coeff.C; - } - else { - ne.q24.A = ne.q24.B = 0; - ne.q24.C = _BV32(24); - } - #endif + E_TERN_(stepper_extruder = current_block->extruder); - // Calculate the initial timer interval - interval = calc_multistep_timer_interval(current_block->initial_rate << oversampling_factor); - // Initialize ac/deceleration time as if half the time passed. - acceleration_time = deceleration_time = interval / 2; - - // Apply Nonlinear Extrusion, if enabled - calc_nonlinear_e(current_block->initial_rate << oversampling_factor); - - #if ENABLED(LIN_ADVANCE) - #if ENABLED(SMOOTH_LIN_ADVANCE) - curr_timer_tick = 0; - #else + // Initialize the trapezoid generator from the current block. + #if HAS_ROUGH_LIN_ADVANCE + #if DISABLED(MIXING_EXTRUDER) && E_STEPPERS > 1 + // If the now active extruder wasn't in use during the last move, its pressure is most likely gone. + if (stepper_extruder != last_moved_extruder) la_advance_steps = 0; + #endif + la_active = (current_block->la_advance_rate != 0); if (la_active) { - const uint32_t la_step_rate = la_advance_steps < current_block->max_adv_steps ? current_block->la_advance_rate : 0; - la_interval = calc_timer_interval((current_block->initial_rate + la_step_rate) >> current_block->la_scaling); + // Apply LA scaling and discount the effect of frequency scaling + la_dividend = (advance_dividend.e << current_block->la_scaling) << oversampling_factor; } #endif - #endif - } - } // !current_block - // Return the interval to wait - return interval; -} + if ( ENABLED(DUAL_X_CARRIAGE) // TODO: Find out why this fixes "jittery" small circles + || current_block->direction_bits != last_direction_bits + || TERN(MIXING_EXTRUDER, false, stepper_extruder != last_moved_extruder) + ) { + E_TERN_(last_moved_extruder = stepper_extruder); + set_directions(current_block->direction_bits); + } + + #if ENABLED(LASER_FEATURE) + if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { // Planner controls the laser + if (planner.laser_inline.status.isSyncPower) + // If the previous block was a M3 sync power then skip the trap power init otherwise it will 0 the sync power. + planner.laser_inline.status.isSyncPower = false; // Clear the flag to process subsequent trap calc's. + else if (current_block->laser.status.isEnabled) { + #if ENABLED(LASER_POWER_TRAP) + TERN_(DEBUG_LASER_TRAP, SERIAL_ECHO_MSG("InitTrapPwr:", current_block->laser.trap_ramp_active_pwr)); + cutter.apply_power(current_block->laser.status.isPowered ? current_block->laser.trap_ramp_active_pwr : 0); + #else + TERN_(DEBUG_CUTTER_POWER, SERIAL_ECHO_MSG("InlinePwr:", current_block->laser.power)); + cutter.apply_power(current_block->laser.status.isPowered ? current_block->laser.power : 0); + #endif + } + } + #endif // LASER_FEATURE + + // If the endstop is already pressed, endstop interrupts won't invoke + // endstop_triggered and the move will grind. So check here for a + // triggered endstop, which marks the block for discard on the next ISR. + endstops.update(); + + #if ENABLED(Z_LATE_ENABLE) + // If delayed Z enable, enable it now. This option will severely interfere with + // timing between pulses when chaining motion between blocks, and it could lead + // to lost steps in both X and Y axis, so avoid using it unless strictly necessary!! + if (current_block->steps.z) enable_axis(Z_AXIS); + #endif + + // Mark ticks_nominal as not-yet-calculated + ticks_nominal = 0; + + #if ENABLED(S_CURVE_ACCELERATION) + // Initialize the Bézier speed curve + _calc_bezier_curve_coeffs(current_block->initial_rate, current_block->cruise_rate, current_block->acceleration_time_inverse); + // We haven't started the 2nd half of the trapezoid + bezier_2nd_half = false; + #else + // Set as deceleration point the initial rate of the block + acc_step_rate = current_block->initial_rate; + #endif + + // Calculate Nonlinear Extrusion fixed-point quotients + #if NONLINEAR_EXTRUSION_Q24 + ne.edividend = advance_dividend.e; + const float scale = (float(ne.edividend) / advance_divisor) * planner.mm_per_step[E_AXIS_N(current_block->extruder)]; + ne.scale_q24 = _BV32(24) * scale; + if (ne.settings.enabled && current_block->direction_bits.e && ANY_AXIS_MOVES(current_block)) { + ne.q24.A = _BV32(24) * ne.settings.coeff.A; + ne.q24.B = _BV32(24) * ne.settings.coeff.B; + ne.q24.C = _BV32(24) * ne.settings.coeff.C; + } + else { + ne.q24.A = ne.q24.B = 0; + ne.q24.C = _BV32(24); + } + #endif + + // Calculate the initial timer interval + interval = calc_multistep_timer_interval(current_block->initial_rate << oversampling_factor); + // Initialize ac/deceleration time as if half the time passed. + acceleration_time = deceleration_time = interval / 2; + + // Apply Nonlinear Extrusion, if enabled + calc_nonlinear_e(current_block->initial_rate << oversampling_factor); + + #if ENABLED(LIN_ADVANCE) + #if ENABLED(SMOOTH_LIN_ADVANCE) + curr_timer_tick = 0; + #else + if (la_active) { + const uint32_t la_step_rate = la_advance_steps < current_block->max_adv_steps ? current_block->la_advance_rate : 0; + la_interval = calc_timer_interval((current_block->initial_rate + la_step_rate) >> current_block->la_scaling); + } + #endif + #endif + } + } // !current_block + + // Return the interval to wait + return interval; + } + +#endif // HAS_STANDARD_MOTION #if ENABLED(LIN_ADVANCE) @@ -3229,12 +3243,12 @@ void Stepper::init() { #define _WRITE_STEP(AXIS, HIGHLOW) AXIS ##_STEP_WRITE(HIGHLOW) #define _DISABLE_AXIS(AXIS) DISABLE_AXIS_## AXIS() - #define AXIS_INIT(AXIS, PIN) \ + #define AXIS_INIT(AXIS, PAXIS) \ _STEP_INIT(AXIS); \ - _WRITE_STEP(AXIS, !_STEP_STATE(PIN)); \ + _WRITE_STEP(AXIS, !_STEP_STATE(PAXIS)); \ _DISABLE_AXIS(AXIS) - #define E_AXIS_INIT(NUM) DEFER(AXIS_INIT)(E##NUM, E) + #define E_AXIS_INIT(NUM) AXIS_INIT(_CAT(E,NUM), E) // Init Step Pins #if HAS_X_STEP diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index b04980cb1e..6e6761f842 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -400,8 +400,11 @@ class Stepper { static block_t* current_block; // A pointer to the block currently being traced - static AxisBits last_direction_bits, // The next stepping-bits to be output - axis_did_move; // Last Movement in the given direction is not null, as computed when the last movement was fetched from planner + static AxisBits last_direction_bits; // The last set of directions applied to all axes + + #if HAS_STANDARD_MOTION + static AxisBits axis_did_move; // Last Movement in the given direction is not null, as computed when the last movement was fetched from planner + #endif static bool abort_current_block; // Signals to the stepper that current block should be aborted @@ -542,11 +545,13 @@ class Stepper { // The ISR scheduler static void isr(); - // The stepper pulse ISR phase - static void pulse_phase_isr(); + #if HAS_STANDARD_MOTION + // The stepper pulse ISR phase + static void pulse_phase_isr(); - // The stepper block processing ISR phase - static hal_timer_t block_phase_isr(); + // The stepper block processing ISR phase + static hal_timer_t block_phase_isr(); + #endif #if HAS_ZV_SHAPING static void shaping_isr(); @@ -618,7 +623,7 @@ class Stepper { if (current_block->is_page()) page_manager.free_page(current_block->page_idx); #endif current_block = nullptr; - axis_did_move.reset(); + TERN_(HAS_STANDARD_MOTION, axis_did_move.reset()); planner.release_current_block(); TERN_(HAS_ROUGH_LIN_ADVANCE, la_interval = nextAdvanceISR = LA_ADV_NEVER); } @@ -629,8 +634,10 @@ class Stepper { // The direction of a single motor. A true result indicates forward or positive motion. FORCE_INLINE static bool motor_direction(const AxisEnum axis) { return last_direction_bits[axis]; } - // The last movement direction was not null on the specified axis. Note that motor direction is not necessarily the same. - FORCE_INLINE static bool axis_is_moving(const AxisEnum axis) { return axis_did_move[axis]; } + #if HAS_STANDARD_MOTION + // The last movement direction was not null on the specified axis. Note that motor direction is not necessarily the same. + FORCE_INLINE static bool axis_is_moving(const AxisEnum axis) { return axis_did_move[axis]; } + #endif // Handle a triggered endstop static void endstop_triggered(const AxisEnum axis); @@ -761,14 +768,14 @@ class Stepper { // Set the current position in steps static void _set_position(const abce_long_t &spos); - // Calculate the timing interval for the given step rate - static hal_timer_t calc_timer_interval(uint32_t step_rate); - - // Calculate timing interval and steps-per-ISR for the given step rate - static hal_timer_t calc_multistep_timer_interval(uint32_t step_rate); - - // Evaluate axis motions and set bits in axis_did_move - static void set_axis_moved_for_current_block(); + #if HAS_STANDARD_MOTION + // Calculate the timing interval for the given step rate + static hal_timer_t calc_timer_interval(uint32_t step_rate); + // Calculate timing interval and steps-per-ISR for the given step rate + static hal_timer_t calc_multistep_timer_interval(uint32_t step_rate); + // Evaluate axis motions and set bits in axis_did_move + static void set_axis_moved_for_current_block(); + #endif #if NONLINEAR_EXTRUSION_Q24 static void calc_nonlinear_e(const uint32_t step_rate); diff --git a/buildroot/tests/rambo b/buildroot/tests/rambo index 6ebb26a177..fcbe648b1d 100755 --- a/buildroot/tests/rambo +++ b/buildroot/tests/rambo @@ -73,7 +73,8 @@ opt_set MOTHERBOARD BOARD_RAMBO EXTRUDERS 0 TEMP_SENSOR_BED 1 TEMP_SENSOR_PROBE DEFAULT_MAX_ACCELERATION '{ 3000, 3000, 100 }' \ MANUAL_FEEDRATE '{ 50*60, 50*60, 4*60 }' \ AXIS_RELATIVE_MODES '{ false, false, false }' -opt_enable REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER FIX_MOUNTED_PROBE Z_SAFE_HOMING FT_MOTION FTM_SMOOTHING FTM_HOME_AND_PROBE +opt_enable REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER FIX_MOUNTED_PROBE Z_SAFE_HOMING \ + FT_MOTION FTM_SMOOTHING FTM_HOME_AND_PROBE NO_STANDARD_MOTION exec_test $1 $2 "Rambo with ZERO EXTRUDERS, heated bed, FT_MOTION" "$3" #