diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 5f5f07d19eb2..f90566f6e22e 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1202,7 +1202,8 @@ */ //#define INPUT_SHAPING_X //#define INPUT_SHAPING_Y -#if ANY(INPUT_SHAPING_X, INPUT_SHAPING_Y) +//#define INPUT_SHAPING_Z +#if ANY(INPUT_SHAPING_X, INPUT_SHAPING_Y, INPUT_SHAPING_Z) #if ENABLED(INPUT_SHAPING_X) #define SHAPING_FREQ_X 40.0 // (Hz) The default dominant resonant frequency on the X axis. #define SHAPING_ZETA_X 0.15 // Damping ratio of the X axis (range: 0.0 = no damping to 1.0 = critical damping). @@ -1211,6 +1212,10 @@ #define SHAPING_FREQ_Y 40.0 // (Hz) The default dominant resonant frequency on the Y axis. #define SHAPING_ZETA_Y 0.15 // Damping ratio of the Y axis (range: 0.0 = no damping to 1.0 = critical damping). #endif + #if ENABLED(INPUT_SHAPING_Z) + #define SHAPING_FREQ_Z 40.0 // (Hz) The default dominant resonant frequency on the Z axis. + #define SHAPING_ZETA_Z 0.15 // Damping ratio of the Z axis (range: 0.0 = no damping to 1.0 = critical damping). + #endif //#define SHAPING_MIN_FREQ 20.0 // (Hz) By default the minimum of the shaping frequencies. Override to affect SRAM usage. //#define SHAPING_MAX_STEPRATE 10000 // By default the maximum total step rate of the shaped axes. Override to affect SRAM usage. //#define SHAPING_MENU // Add a menu to the LCD to set shaping parameters. diff --git a/Marlin/src/gcode/feature/input_shaping/M593.cpp b/Marlin/src/gcode/feature/input_shaping/M593.cpp index 249536efffb5..6ce942e4eefe 100644 --- a/Marlin/src/gcode/feature/input_shaping/M593.cpp +++ b/Marlin/src/gcode/feature/input_shaping/M593.cpp @@ -44,6 +44,15 @@ void GcodeSuite::M593_report(const bool forReplay/*=true*/) { " D", stepper.get_shaping_damping_ratio(Y_AXIS) ); #endif + #if ENABLED(INPUT_SHAPING_Z) + #if ANY(INPUT_SHAPING_X, INPUT_SHAPING_Y) + report_echo_start(forReplay); + #endif + SERIAL_ECHOLNPGM(" M593 Z" + " F", stepper.get_shaping_frequency(Z_AXIS), + " D", stepper.get_shaping_damping_ratio(Z_AXIS) + ); + #endif } /** @@ -59,14 +68,17 @@ void GcodeSuite::M593() { const bool seen_X = TERN0(INPUT_SHAPING_X, parser.seen_test('X')), seen_Y = TERN0(INPUT_SHAPING_Y, parser.seen_test('Y')), - for_X = seen_X || TERN0(INPUT_SHAPING_X, (!seen_X && !seen_Y)), - for_Y = seen_Y || TERN0(INPUT_SHAPING_Y, (!seen_X && !seen_Y)); + seen_Z = TERN0(INPUT_SHAPING_Z, parser.seen_test('Z')), + for_X = seen_X || TERN0(INPUT_SHAPING_X, (!seen_X && !seen_Y && !seen_Z)), + for_Y = seen_Y || TERN0(INPUT_SHAPING_Y, (!seen_X && !seen_Y && !seen_Z)), + for_Z = seen_Z || TERN0(INPUT_SHAPING_Z, (!seen_X && !seen_Y && !seen_Z)); if (parser.seen('D')) { const float zeta = parser.value_float(); if (WITHIN(zeta, 0, 1)) { if (for_X) stepper.set_shaping_damping_ratio(X_AXIS, zeta); if (for_Y) stepper.set_shaping_damping_ratio(Y_AXIS, zeta); + if (for_Z) stepper.set_shaping_damping_ratio(Z_AXIS, zeta); } else SERIAL_ECHO_MSG("?Zeta (D) value out of range (0-1)"); @@ -78,6 +90,7 @@ void GcodeSuite::M593() { if (freq == 0.0f || freq > min_freq) { if (for_X) stepper.set_shaping_frequency(X_AXIS, freq); if (for_Y) stepper.set_shaping_frequency(Y_AXIS, freq); + if (for_Z) stepper.set_shaping_frequency(Z_AXIS, freq); } else SERIAL_ECHOLNPGM(GCODE_ERR_MSG("Frequency (F) must be greater than ", min_freq, " or 0 to disable")); diff --git a/Marlin/src/inc/Conditionals_adv.h b/Marlin/src/inc/Conditionals_adv.h index b48782e17b9b..aa185db25842 100644 --- a/Marlin/src/inc/Conditionals_adv.h +++ b/Marlin/src/inc/Conditionals_adv.h @@ -112,8 +112,8 @@ #undef DISABLE_IDLE_X #undef INPUT_SHAPING_X #undef SAFE_BED_LEVELING_START_X - #undef SHAPING_BUFFER_X #undef SHAPING_FREQ_X + #undef SHAPING_ZETA_X #undef STEALTHCHOP_X #endif @@ -128,8 +128,8 @@ #undef INPUT_SHAPING_Y #undef QUICK_HOME #undef SAFE_BED_LEVELING_START_Y - #undef SHAPING_BUFFER_Y #undef SHAPING_FREQ_Y + #undef SHAPING_ZETA_Y #undef STEALTHCHOP_Y #undef STEP_STATE_Y #endif @@ -142,8 +142,11 @@ #undef ENABLE_LEVELING_FADE_HEIGHT #undef HOME_Z_FIRST #undef HOMING_Z_WITH_PROBE + #undef INPUT_SHAPING_Z #undef NUM_Z_STEPPERS #undef SAFE_BED_LEVELING_START_Z + #undef SHAPING_FREQ_Z + #undef SHAPING_ZETA_Z #undef STEALTHCHOP_Z #undef STEP_STATE_Z #undef Z_IDLE_HEIGHT @@ -1338,7 +1341,7 @@ #endif // Input shaping -#if ANY(INPUT_SHAPING_X, INPUT_SHAPING_Y) +#if ANY(INPUT_SHAPING_X, INPUT_SHAPING_Y, INPUT_SHAPING_Z) #define HAS_ZV_SHAPING 1 #endif diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index 665a7306f8dc..0383d29a690a 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -4208,7 +4208,12 @@ static_assert(_PLUS_TEST(3), "DEFAULT_MAX_ACCELERATION values must be positive." */ #if HAS_ZV_SHAPING #if ENABLED(DELTA) - #error "Input Shaping is not compatible with DELTA kinematics." + #if !ALL(INPUT_SHAPING_X, INPUT_SHAPING_Y, INPUT_SHAPING_Z) + #error "INPUT_SHAPING_X, INPUT_SHAPING_Y and INPUT_SHAPING_Z must all be enabled for DELTA." + #else + static_assert(SHAPING_FREQ_X == SHAPING_FREQ_Y && SHAPING_FREQ_Y == SHAPING_FREQ_Z, "SHAPING_FREQ_X, SHAPING_FREQ_Y and SHAPING_FREQ_Z must be the same for DELTA."); + static_assert(SHAPING_ZETA_X == SHAPING_ZETA_Y && SHAPING_ZETA_Y == SHAPING_ZETA_Z, "SHAPING_ZETA_X, SHAPING_ZETA_Y and SHAPING_ZETA_Z must be the same for DELTA."); + #endif #elif ENABLED(SCARA) #error "Input Shaping is not compatible with SCARA kinematics." #elif ENABLED(TPARA) @@ -4220,9 +4225,19 @@ static_assert(_PLUS_TEST(3), "DEFAULT_MAX_ACCELERATION values must be positive." #elif ENABLED(DIRECT_STEPPING) #error "Input Shaping is not compatible with DIRECT_STEPPING." #elif ALL(INPUT_SHAPING_X, CORE_IS_XZ) - #error "INPUT_SHAPING_X is not supported with COREXZ." + #if !ALL(INPUT_SHAPING_X, INPUT_SHAPING_Z) + #error "INPUT_SHAPING_X and INPUT_SHAPING_Z must both be enabled for COREXZ." + #else + static_assert(SHAPING_FREQ_X == SHAPING_FREQ_Z, "SHAPING_FREQ_X and SHAPING_FREQ_Z must be the same for COREXZ."); + static_assert(SHAPING_ZETA_X == SHAPING_ZETA_Z, "SHAPING_ZETA_X and SHAPING_ZETA_Z must be the same for COREXZ."); + #endif #elif ALL(INPUT_SHAPING_Y, CORE_IS_YZ) - #error "INPUT_SHAPING_Y is not supported with COREYZ." + #if !ALL(INPUT_SHAPING_Y, INPUT_SHAPING_Z) + #error "INPUT_SHAPING_Y and INPUT_SHAPING_Z must both be enabled for COREYZ." + #else + static_assert(SHAPING_FREQ_Y == SHAPING_FREQ_Z, "SHAPING_FREQ_Y and SHAPING_FREQ_Z must be the same for COREYZ."); + static_assert(SHAPING_ZETA_Y == SHAPING_ZETA_Z, "SHAPING_ZETA_Y and SHAPING_ZETA_Z must be the same for COREYZ."); + #endif #elif ANY(CORE_IS_XY, MARKFORGED_XY, MARKFORGED_YX) #if !ALL(INPUT_SHAPING_X, INPUT_SHAPING_Y) #error "INPUT_SHAPING_X and INPUT_SHAPING_Y must both be enabled for COREXY, COREYX, or MARKFORGED_*." @@ -4237,6 +4252,7 @@ static_assert(_PLUS_TEST(3), "DEFAULT_MAX_ACCELERATION values must be positive." #else TERN_(INPUT_SHAPING_X, static_assert((SHAPING_FREQ_X) > 0, "SHAPING_FREQ_X must be > 0 or SHAPING_MIN_FREQ must be set.")); TERN_(INPUT_SHAPING_Y, static_assert((SHAPING_FREQ_Y) > 0, "SHAPING_FREQ_Y must be > 0 or SHAPING_MIN_FREQ must be set.")); + TERN_(INPUT_SHAPING_Z, static_assert((SHAPING_FREQ_Z) > 0, "SHAPING_FREQ_Z must be > 0 or SHAPING_MIN_FREQ must be set.")); #endif #ifdef __AVR__ #if ENABLED(INPUT_SHAPING_X) @@ -4253,6 +4269,13 @@ static_assert(_PLUS_TEST(3), "DEFAULT_MAX_ACCELERATION values must be positive." static_assert((SHAPING_FREQ_Y) == 0 || (SHAPING_FREQ_Y) * 2 * 0x10000 >= (STEPPER_TIMER_RATE), "SHAPING_FREQ_Y is below the minimum (16) for AVR 16MHz."); #endif #endif + #if ENABLED(INPUT_SHAPING_Z) + #if F_CPU > 16000000 + static_assert((SHAPING_FREQ_Z) == 0 || (SHAPING_FREQ_Z) * 2 * 0x10000 >= (STEPPER_TIMER_RATE), "SHAPING_FREQ_Z is below the minimum (20) for AVR 20MHz."); + #else + static_assert((SHAPING_FREQ_Z) == 0 || (SHAPING_FREQ_Z) * 2 * 0x10000 >= (STEPPER_TIMER_RATE), "SHAPING_FREQ_Z is below the minimum (16) for AVR 16MHz."); + #endif + #endif #endif #endif diff --git a/Marlin/src/lcd/e3v2/proui/dwin.cpp b/Marlin/src/lcd/e3v2/proui/dwin.cpp index debe3b5de02e..18c97adb3a3e 100644 --- a/Marlin/src/lcd/e3v2/proui/dwin.cpp +++ b/Marlin/src/lcd/e3v2/proui/dwin.cpp @@ -3478,6 +3478,13 @@ void drawTuneMenu() { void setShapingYZeta() { hmiValue.axis = Y_AXIS; setFloatOnClick(0, 1, 2, stepper.get_shaping_damping_ratio(Y_AXIS), applyShapingZeta); } #endif + #if ENABLED(INPUT_SHAPING_Z) + void onDrawShapingZFreq(MenuItem* menuitem, int8_t line) { onDrawFloatMenu(menuitem, line, 2, stepper.get_shaping_frequency(Z_AXIS)); } + void onDrawShapingZZeta(MenuItem* menuitem, int8_t line) { onDrawFloatMenu(menuitem, line, 2, stepper.get_shaping_damping_ratio(Z_AXIS)); } + void setShapingZFreq() { hmiValue.axis = Z_AXIS; setFloatOnClick(0, 200, 2, stepper.get_shaping_frequency(Z_AXIS), applyShapingFreq); } + void setShapingZZeta() { hmiValue.axis = Z_AXIS; setFloatOnClick(0, 1, 2, stepper.get_shaping_damping_ratio(Z_AXIS), applyShapingZeta); } + #endif + void drawInputShaping_menu() { checkkey = ID_Menu; if (SET_MENU(inputShapingMenu, MSG_INPUT_SHAPING, 5)) { @@ -3490,6 +3497,10 @@ void drawTuneMenu() { MENU_ITEM(ICON_ShapingY, MSG_SHAPING_B_FREQ, onDrawShapingYFreq, setShapingYFreq); MENU_ITEM(ICON_ShapingY, MSG_SHAPING_B_ZETA, onDrawShapingYZeta, setShapingYZeta); #endif + #if ENABLED(INPUT_SHAPING_Z) + MENU_ITEM(ICON_ShapingZ, MSG_SHAPING_C_FREQ, onDrawShapingZFreq, setShapingZFreq); + MENU_ITEM(ICON_ShapingZ, MSG_SHAPING_C_ZETA, onDrawShapingZZeta, setShapingZZeta); + #endif } updateMenu(inputShapingMenu); } diff --git a/Marlin/src/lcd/menu/menu_advanced.cpp b/Marlin/src/lcd/menu/menu_advanced.cpp index 38d56bf912b0..b7825949c07a 100644 --- a/Marlin/src/lcd/menu/menu_advanced.cpp +++ b/Marlin/src/lcd/menu/menu_advanced.cpp @@ -559,28 +559,20 @@ void menu_backlash(); BACK_ITEM(MSG_ADVANCED_SETTINGS); // M593 F Frequency and D Damping ratio - #if ENABLED(INPUT_SHAPING_X) - editable.decimal = stepper.get_shaping_frequency(X_AXIS); - if (editable.decimal) { - ACTION_ITEM_N(X_AXIS, MSG_SHAPING_DISABLE, []{ stepper.set_shaping_frequency(X_AXIS, 0.0f); ui.refresh(); }); - EDIT_ITEM_FAST_N(float41, X_AXIS, MSG_SHAPING_FREQ, &editable.decimal, min_frequency, 200.0f, []{ stepper.set_shaping_frequency(X_AXIS, editable.decimal); }); - editable.decimal = stepper.get_shaping_damping_ratio(X_AXIS); - EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_SHAPING_ZETA, &editable.decimal, 0.0f, 1.0f, []{ stepper.set_shaping_damping_ratio(X_AXIS, editable.decimal); }); - } - else - ACTION_ITEM_N(X_AXIS, MSG_SHAPING_ENABLE, []{ stepper.set_shaping_frequency(X_AXIS, (SHAPING_FREQ_X) ?: (SHAPING_MIN_FREQ)); ui.refresh(); }); - #endif - #if ENABLED(INPUT_SHAPING_Y) - editable.decimal = stepper.get_shaping_frequency(Y_AXIS); - if (editable.decimal) { - ACTION_ITEM_N(Y_AXIS, MSG_SHAPING_DISABLE, []{ stepper.set_shaping_frequency(Y_AXIS, 0.0f); ui.refresh(); }); - EDIT_ITEM_FAST_N(float41, Y_AXIS, MSG_SHAPING_FREQ, &editable.decimal, min_frequency, 200.0f, []{ stepper.set_shaping_frequency(Y_AXIS, editable.decimal); }); - editable.decimal = stepper.get_shaping_damping_ratio(Y_AXIS); - EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_SHAPING_ZETA, &editable.decimal, 0.0f, 1.0f, []{ stepper.set_shaping_damping_ratio(Y_AXIS, editable.decimal); }); - } - else - ACTION_ITEM_N(Y_AXIS, MSG_SHAPING_ENABLE, []{ stepper.set_shaping_frequency(Y_AXIS, (SHAPING_FREQ_Y) ?: (SHAPING_MIN_FREQ)); ui.refresh(); }); - #endif + #define SHAPING_MENU_FOR_AXIS(AXIS) \ + editable.decimal = stepper.get_shaping_frequency(AXIS); \ + if (editable.decimal) { \ + ACTION_ITEM_N(AXIS, MSG_SHAPING_DISABLE, []{ stepper.set_shaping_frequency(AXIS, 0.0f); ui.refresh(); }); \ + EDIT_ITEM_FAST_N(float41, AXIS, MSG_SHAPING_FREQ, &editable.decimal, min_frequency, 200.0f, []{ stepper.set_shaping_frequency(AXIS, editable.decimal); }); \ + editable.decimal = stepper.get_shaping_damping_ratio(AXIS); \ + EDIT_ITEM_FAST_N(float42_52, AXIS, MSG_SHAPING_ZETA, &editable.decimal, 0.0f, 1.0f, []{ stepper.set_shaping_damping_ratio(AXIS, editable.decimal); }); \ + } \ + else \ + ACTION_ITEM_N(AXIS, MSG_SHAPING_ENABLE, []{ stepper.set_shaping_frequency(AXIS, (SHAPING_FREQ_X) ?: (SHAPING_MIN_FREQ)); ui.refresh(); }); + + TERN_(INPUT_SHAPING_X, SHAPING_MENU_FOR_AXIS(X_AXIS)) + TERN_(INPUT_SHAPING_Y, SHAPING_MENU_FOR_AXIS(Y_AXIS)) + TERN_(INPUT_SHAPING_Z, SHAPING_MENU_FOR_AXIS(Z_AXIS)) END_MENU(); } diff --git a/Marlin/src/module/settings.cpp b/Marlin/src/module/settings.cpp index 17b8ae4d72fa..a9f2f888bff4 100644 --- a/Marlin/src/module/settings.cpp +++ b/Marlin/src/module/settings.cpp @@ -634,6 +634,10 @@ typedef struct SettingsDataStruct { float shaping_y_frequency, // M593 Y F shaping_y_zeta; // M593 Y D #endif + #if ENABLED(INPUT_SHAPING_Z) + float shaping_z_frequency, // M593 Z F + shaping_z_zeta; // M593 Z D + #endif // // HOTEND_IDLE_TIMEOUT @@ -1731,6 +1735,10 @@ void MarlinSettings::postprocess() { EEPROM_WRITE(stepper.get_shaping_frequency(Y_AXIS)); EEPROM_WRITE(stepper.get_shaping_damping_ratio(Y_AXIS)); #endif + #if ENABLED(INPUT_SHAPING_Z) + EEPROM_WRITE(stepper.get_shaping_frequency(Z_AXIS)); + EEPROM_WRITE(stepper.get_shaping_damping_ratio(Z_AXIS)); + #endif #endif // @@ -2813,22 +2821,33 @@ void MarlinSettings::postprocess() { // #if ENABLED(INPUT_SHAPING_X) { - float _data[2]; + struct { float freq, damp; } _data; EEPROM_READ(_data); if (!validating) { - stepper.set_shaping_frequency(X_AXIS, _data[0]); - stepper.set_shaping_damping_ratio(X_AXIS, _data[1]); + stepper.set_shaping_frequency(X_AXIS, _data.freq); + stepper.set_shaping_damping_ratio(X_AXIS, _data.damp); } } #endif #if ENABLED(INPUT_SHAPING_Y) { - float _data[2]; + struct { float freq, damp; } _data; + EEPROM_READ(_data); + if (!validating) { + stepper.set_shaping_frequency(Y_AXIS, _data.freq); + stepper.set_shaping_damping_ratio(Y_AXIS, _data.damp); + } + } + #endif + + #if ENABLED(INPUT_SHAPING_Z) + { + struct { float freq, damp; } _data; EEPROM_READ(_data); if (!validating) { - stepper.set_shaping_frequency(Y_AXIS, _data[0]); - stepper.set_shaping_damping_ratio(Y_AXIS, _data[1]); + stepper.set_shaping_frequency(Z_AXIS, _data.freq); + stepper.set_shaping_damping_ratio(Z_AXIS, _data.damp); } } #endif @@ -3665,6 +3684,10 @@ void MarlinSettings::reset() { stepper.set_shaping_frequency(Y_AXIS, SHAPING_FREQ_Y); stepper.set_shaping_damping_ratio(Y_AXIS, SHAPING_ZETA_Y); #endif + #if ENABLED(INPUT_SHAPING_Z) + stepper.set_shaping_frequency(Z_AXIS, SHAPING_FREQ_Z); + stepper.set_shaping_damping_ratio(Z_AXIS, SHAPING_ZETA_Z); + #endif #endif // diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index c2674480d52a..083d2019dc33 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -281,20 +281,16 @@ uint32_t Stepper::advance_divisor = 0, shaping_echo_axis_t ShapingQueue::echo_axes[shaping_echoes]; uint16_t ShapingQueue::tail = 0; - #if ENABLED(INPUT_SHAPING_X) - shaping_time_t ShapingQueue::delay_x; - shaping_time_t ShapingQueue::peek_x_val = shaping_time_t(-1); - uint16_t ShapingQueue::head_x = 0; - uint16_t ShapingQueue::_free_count_x = shaping_echoes - 1; - ShapeParams Stepper::shaping_x; - #endif - #if ENABLED(INPUT_SHAPING_Y) - shaping_time_t ShapingQueue::delay_y; - shaping_time_t ShapingQueue::peek_y_val = shaping_time_t(-1); - uint16_t ShapingQueue::head_y = 0; - uint16_t ShapingQueue::_free_count_y = shaping_echoes - 1; - ShapeParams Stepper::shaping_y; - #endif + #define SHAPING_VAR_DEFS(AXIS) \ + shaping_time_t ShapingQueue::delay_##AXIS; \ + shaping_time_t ShapingQueue::_peek_##AXIS = shaping_time_t(-1); \ + uint16_t ShapingQueue::head_##AXIS = 0; \ + uint16_t ShapingQueue::_free_count_##AXIS = shaping_echoes - 1; \ + ShapeParams Stepper::shaping_##AXIS; + + TERN_(INPUT_SHAPING_X, SHAPING_VAR_DEFS(x)) + TERN_(INPUT_SHAPING_Y, SHAPING_VAR_DEFS(y)) + TERN_(INPUT_SHAPING_Z, SHAPING_VAR_DEFS(z)) #endif #if ENABLED(BABYSTEPPING) @@ -1610,6 +1606,7 @@ void Stepper::isr() { interval = _MIN(nextMainISR, uint32_t(HAL_TIMER_TYPE_MAX)); // 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_(BABYSTEPPING, NOMORE(interval, nextBabystepISR)); // Come back early for Babystepping? @@ -1754,6 +1751,10 @@ void Stepper::pulse_phase_isr() { 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 } } @@ -1813,6 +1814,12 @@ void Stepper::pulse_phase_isr() { #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) @@ -2005,9 +2012,10 @@ void Stepper::pulse_phase_isr() { #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); - if (x_step || y_step) - ShapingQueue::enqueue(x_step, TERN0(INPUT_SHAPING_X, shaping_x.forward), y_step, TERN0(INPUT_SHAPING_Y, shaping_y.forward)); + 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) @@ -2018,6 +2026,10 @@ void Stepper::pulse_phase_isr() { 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 } @@ -2124,6 +2136,7 @@ void Stepper::pulse_phase_isr() { // Clear the echoes that are ready to process. If the buffers are too full and risk overflow, also apply echoes early. TERN_(INPUT_SHAPING_X, step_needed.x = !ShapingQueue::peek_x() || ShapingQueue::free_count_x() < steps_per_isr); TERN_(INPUT_SHAPING_Y, step_needed.y = !ShapingQueue::peek_y() || ShapingQueue::free_count_y() < steps_per_isr); + TERN_(INPUT_SHAPING_Z, step_needed.z = !ShapingQueue::peek_z() || ShapingQueue::free_count_z() < steps_per_isr); if (bool(step_needed)) while (true) { #if ENABLED(INPUT_SHAPING_X) @@ -2142,6 +2155,14 @@ void Stepper::pulse_phase_isr() { } #endif + #if ENABLED(INPUT_SHAPING_Z) + if (step_needed.z) { + const bool forward = ShapingQueue::dequeue_z(); + PULSE_PREP_SHAPING(Z, shaping_z.delta_error, (forward ? shaping_z.factor2 : -shaping_z.factor2)); + PULSE_START(Z); + } + #endif + TERN_(I2S_STEPPER_STREAM, i2s_push_sample()); USING_TIMED_PULSE(); @@ -2156,10 +2177,14 @@ void Stepper::pulse_phase_isr() { #if ENABLED(INPUT_SHAPING_Y) PULSE_STOP(Y); #endif + #if ENABLED(INPUT_SHAPING_Z) + PULSE_STOP(Z); + #endif } TERN_(INPUT_SHAPING_X, step_needed.x = !ShapingQueue::peek_x() || ShapingQueue::free_count_x() < steps_per_isr); TERN_(INPUT_SHAPING_Y, step_needed.y = !ShapingQueue::peek_y() || ShapingQueue::free_count_y() < steps_per_isr); + TERN_(INPUT_SHAPING_Z, step_needed.z = !ShapingQueue::peek_z() || ShapingQueue::free_count_z() < steps_per_isr); if (!bool(step_needed)) break; @@ -2708,7 +2733,7 @@ hal_timer_t Stepper::block_phase_isr() { } #endif - // Y follows the same logic as X (but the comments aren't repeated) + // 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); @@ -2718,6 +2743,15 @@ hal_timer_t Stepper::block_phase_isr() { } #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; @@ -3220,12 +3254,14 @@ void Stepper::init() { hal.isr_off(); TERN_(INPUT_SHAPING_X, if (axis == X_AXIS) { shaping_x.factor2 = factor2; shaping_x.factor1 = 128 - factor2; shaping_x.zeta = zeta; }) TERN_(INPUT_SHAPING_Y, if (axis == Y_AXIS) { shaping_y.factor2 = factor2; shaping_y.factor1 = 128 - factor2; shaping_y.zeta = zeta; }) + TERN_(INPUT_SHAPING_Z, if (axis == Z_AXIS) { shaping_z.factor2 = factor2; shaping_z.factor1 = 128 - factor2; shaping_z.zeta = zeta; }) if (was_on) hal.isr_on(); } float Stepper::get_shaping_damping_ratio(const AxisEnum axis) { TERN_(INPUT_SHAPING_X, if (axis == X_AXIS) return shaping_x.zeta); TERN_(INPUT_SHAPING_Y, if (axis == Y_AXIS) return shaping_y.zeta); + TERN_(INPUT_SHAPING_Z, if (axis == Z_AXIS) return shaping_z.zeta); return -1; } @@ -3237,24 +3273,18 @@ void Stepper::init() { hal.isr_off(); const shaping_time_t delay = freq ? float(uint32_t(STEPPER_TIMER_RATE) / 2) / freq : shaping_time_t(-1); - #if ENABLED(INPUT_SHAPING_X) - if (axis == X_AXIS) { - ShapingQueue::set_delay(X_AXIS, delay); - shaping_x.frequency = freq; - shaping_x.enabled = !!freq; - shaping_x.delta_error = 0; - shaping_x.last_block_end_pos = count_position.x; - } - #endif - #if ENABLED(INPUT_SHAPING_Y) - if (axis == Y_AXIS) { - ShapingQueue::set_delay(Y_AXIS, delay); - shaping_y.frequency = freq; - shaping_y.enabled = !!freq; - shaping_y.delta_error = 0; - shaping_y.last_block_end_pos = count_position.y; + #define SHAPING_SET_FREQ_FOR_AXIS(AXISN, AXISL) \ + if (axis == AXISN) { \ + ShapingQueue::set_delay(AXISN, delay); \ + shaping_##AXISL.frequency = freq; \ + shaping_##AXISL.enabled = !!freq; \ + shaping_##AXISL.delta_error = 0; \ + shaping_##AXISL.last_block_end_pos = count_position.AXISL; \ } - #endif + + TERN_(INPUT_SHAPING_X, SHAPING_SET_FREQ_FOR_AXIS(X_AXIS, x)) + TERN_(INPUT_SHAPING_Y, SHAPING_SET_FREQ_FOR_AXIS(Y_AXIS, y)) + TERN_(INPUT_SHAPING_Z, SHAPING_SET_FREQ_FOR_AXIS(Z_AXIS, z)) if (was_on) hal.isr_on(); } @@ -3262,6 +3292,7 @@ void Stepper::init() { float Stepper::get_shaping_frequency(const AxisEnum axis) { TERN_(INPUT_SHAPING_X, if (axis == X_AXIS) return shaping_x.frequency); TERN_(INPUT_SHAPING_Y, if (axis == Y_AXIS) return shaping_y.frequency); + TERN_(INPUT_SHAPING_Z, if (axis == Z_AXIS) return shaping_z.frequency); return -1; } @@ -3283,6 +3314,9 @@ void Stepper::_set_position(const abce_long_t &spos) { #if ENABLED(INPUT_SHAPING_Y) const int32_t y_shaping_delta = count_position.y - shaping_y.last_block_end_pos; #endif + #if ENABLED(INPUT_SHAPING_Z) + const int32_t z_shaping_delta = count_position.z - shaping_z.last_block_end_pos; + #endif #if ANY(IS_CORE, MARKFORGED_XY, MARKFORGED_YX) // Core equations follow the form of the dA and dB equations at https://www.corexy.com/theory.html @@ -3323,6 +3357,12 @@ void Stepper::_set_position(const abce_long_t &spos) { shaping_y.last_block_end_pos = spos.y; } #endif + #if ENABLED(INPUT_SHAPING_Z) + if (shaping_z.enabled) { + count_position.z += z_shaping_delta; + shaping_z.last_block_end_pos = spos.z; + } + #endif } /** @@ -3364,6 +3404,7 @@ void Stepper::set_axis_position(const AxisEnum a, const int32_t &v) { count_position[a] = v; TERN_(INPUT_SHAPING_X, if (a == X_AXIS) shaping_x.last_block_end_pos = v); TERN_(INPUT_SHAPING_Y, if (a == Y_AXIS) shaping_y.last_block_end_pos = v); + TERN_(INPUT_SHAPING_Z, if (a == Z_AXIS) shaping_z.last_block_end_pos = v); #ifdef __AVR__ // Reenable Stepper ISR diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 82b41290bf14..8cf6d39deac3 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -143,7 +143,8 @@ constexpr ena_mask_t enable_overlap[] = { constexpr float _ISDASU[] = DEFAULT_AXIS_STEPS_PER_UNIT; constexpr feedRate_t _ISDMF[] = DEFAULT_MAX_FEEDRATE; constexpr float max_shaped_rate = TERN0(INPUT_SHAPING_X, _ISDMF[X_AXIS] * _ISDASU[X_AXIS]) + - TERN0(INPUT_SHAPING_Y, _ISDMF[Y_AXIS] * _ISDASU[Y_AXIS]); + TERN0(INPUT_SHAPING_Y, _ISDMF[Y_AXIS] * _ISDASU[Y_AXIS]) + + TERN0(INPUT_SHAPING_Z, _ISDMF[Z_AXIS] * _ISDASU[Z_AXIS]); #if defined(__AVR__) || !defined(ADAPTIVE_STEP_SMOOTHING) // MIN_STEP_ISR_FREQUENCY is known at compile time on AVRs and any reduction in SRAM is welcome template constexpr float max_isr_rate() { @@ -159,7 +160,7 @@ constexpr ena_mask_t enable_overlap[] = { #endif #ifndef SHAPING_MIN_FREQ - #define SHAPING_MIN_FREQ _MIN(__FLT_MAX__ OPTARG(INPUT_SHAPING_X, SHAPING_FREQ_X) OPTARG(INPUT_SHAPING_Y, SHAPING_FREQ_Y)) + #define SHAPING_MIN_FREQ _MIN(__FLT_MAX__ OPTARG(INPUT_SHAPING_X, SHAPING_FREQ_X) OPTARG(INPUT_SHAPING_Y, SHAPING_FREQ_Y) OPTARG(INPUT_SHAPING_Z, SHAPING_FREQ_Z)) #endif constexpr float shaping_min_freq = SHAPING_MIN_FREQ; constexpr uint16_t shaping_echoes = FLOOR(max_step_rate / shaping_min_freq / 2) + 3; @@ -169,6 +170,7 @@ constexpr ena_mask_t enable_overlap[] = { struct shaping_echo_axis_t { TERN_(INPUT_SHAPING_X, shaping_echo_t x:2); TERN_(INPUT_SHAPING_Y, shaping_echo_t y:2); + TERN_(INPUT_SHAPING_Z, shaping_echo_t z:2); }; class ShapingQueue { @@ -178,96 +180,89 @@ constexpr ena_mask_t enable_overlap[] = { static shaping_echo_axis_t echo_axes[shaping_echoes]; static uint16_t tail; - #if ENABLED(INPUT_SHAPING_X) - static shaping_time_t delay_x; // = shaping_time_t(-1) to disable queueing - static shaping_time_t peek_x_val; - static uint16_t head_x; - static uint16_t _free_count_x; - #endif - #if ENABLED(INPUT_SHAPING_Y) - static shaping_time_t delay_y; // = shaping_time_t(-1) to disable queueing - static shaping_time_t peek_y_val; - static uint16_t head_y; - static uint16_t _free_count_y; - #endif + #define SHAPING_QUEUE_AXIS_VARS(AXIS) \ + static shaping_time_t delay_##AXIS; /* = shaping_time_t(-1) to disable queueing*/ \ + static shaping_time_t _peek_##AXIS; \ + static uint16_t head_##AXIS; \ + static uint16_t _free_count_##AXIS; + + TERN_(INPUT_SHAPING_X, SHAPING_QUEUE_AXIS_VARS(x)) + TERN_(INPUT_SHAPING_Y, SHAPING_QUEUE_AXIS_VARS(y)) + TERN_(INPUT_SHAPING_Z, SHAPING_QUEUE_AXIS_VARS(z)) public: static void decrement_delays(const shaping_time_t interval) { now += interval; - TERN_(INPUT_SHAPING_X, if (peek_x_val != shaping_time_t(-1)) peek_x_val -= interval); - TERN_(INPUT_SHAPING_Y, if (peek_y_val != shaping_time_t(-1)) peek_y_val -= interval); + TERN_(INPUT_SHAPING_X, if (_peek_x != shaping_time_t(-1)) _peek_x -= interval); + TERN_(INPUT_SHAPING_Y, if (_peek_y != shaping_time_t(-1)) _peek_y -= interval); + TERN_(INPUT_SHAPING_Z, if (_peek_z != shaping_time_t(-1)) _peek_z -= interval); } static void set_delay(const AxisEnum axis, const shaping_time_t delay) { TERN_(INPUT_SHAPING_X, if (axis == X_AXIS) delay_x = delay); TERN_(INPUT_SHAPING_Y, if (axis == Y_AXIS) delay_y = delay); + TERN_(INPUT_SHAPING_Z, if (axis == Z_AXIS) delay_z = delay); } - static void enqueue(const bool x_step, const bool x_forward, const bool y_step, const bool y_forward) { - #if ENABLED(INPUT_SHAPING_X) - if (x_step) { - if (head_x == tail) peek_x_val = delay_x; - echo_axes[tail].x = x_forward ? ECHO_FWD : ECHO_BWD; - _free_count_x--; - } - else { - echo_axes[tail].x = ECHO_NONE; - if (head_x != tail) - _free_count_x--; - else if (++head_x == shaping_echoes) - head_x = 0; - } - #endif - #if ENABLED(INPUT_SHAPING_Y) - if (y_step) { - if (head_y == tail) peek_y_val = delay_y; - echo_axes[tail].y = y_forward ? ECHO_FWD : ECHO_BWD; - _free_count_y--; - } - else { - echo_axes[tail].y = ECHO_NONE; - if (head_y != tail) - _free_count_y--; - else if (++head_y == shaping_echoes) - head_y = 0; + + static void enqueue(const bool x_step, const bool x_forward, const bool y_step, const bool y_forward, const bool z_step, const bool z_forward) { + #define SHAPING_QUEUE_ENQUEUE(AXIS) \ + if (AXIS##_step) { \ + if (head_##AXIS == tail) _peek_##AXIS = delay_##AXIS; \ + echo_axes[tail].AXIS = AXIS##_forward ? ECHO_FWD : ECHO_BWD; \ + _free_count_##AXIS--; \ + } \ + else { \ + echo_axes[tail].AXIS = ECHO_NONE; \ + if (head_##AXIS != tail) \ + _free_count_##AXIS--; \ + else if (++head_##AXIS == shaping_echoes) \ + head_##AXIS = 0; \ } - #endif + + TERN_(INPUT_SHAPING_X, SHAPING_QUEUE_ENQUEUE(x)) + TERN_(INPUT_SHAPING_Y, SHAPING_QUEUE_ENQUEUE(y)) + TERN_(INPUT_SHAPING_Z, SHAPING_QUEUE_ENQUEUE(z)) + times[tail] = now; if (++tail == shaping_echoes) tail = 0; } + + #define SHAPING_QUEUE_DEQUEUE(AXIS) \ + bool forward = echo_axes[head_##AXIS].AXIS == ECHO_FWD; \ + do { \ + _free_count_##AXIS++; \ + if (++head_##AXIS == shaping_echoes) head_##AXIS = 0; \ + } while (head_##AXIS != tail && echo_axes[head_##AXIS].AXIS == ECHO_NONE); \ + _peek_##AXIS = head_##AXIS == tail ? shaping_time_t(-1) : times[head_##AXIS] + delay_##AXIS - now; \ + return forward; + #if ENABLED(INPUT_SHAPING_X) - static shaping_time_t peek_x() { return peek_x_val; } - static bool dequeue_x() { - bool forward = echo_axes[head_x].x == ECHO_FWD; - do { - _free_count_x++; - if (++head_x == shaping_echoes) head_x = 0; - } while (head_x != tail && echo_axes[head_x].x == ECHO_NONE); - peek_x_val = head_x == tail ? shaping_time_t(-1) : times[head_x] + delay_x - now; - return forward; - } + static shaping_time_t peek_x() { return _peek_x; } + static bool dequeue_x() { SHAPING_QUEUE_DEQUEUE(x) } static bool empty_x() { return head_x == tail; } static uint16_t free_count_x() { return _free_count_x; } #endif #if ENABLED(INPUT_SHAPING_Y) - static shaping_time_t peek_y() { return peek_y_val; } - static bool dequeue_y() { - bool forward = echo_axes[head_y].y == ECHO_FWD; - do { - _free_count_y++; - if (++head_y == shaping_echoes) head_y = 0; - } while (head_y != tail && echo_axes[head_y].y == ECHO_NONE); - peek_y_val = head_y == tail ? shaping_time_t(-1) : times[head_y] + delay_y - now; - return forward; - } + static shaping_time_t peek_y() { return _peek_y; } + static bool dequeue_y() { SHAPING_QUEUE_DEQUEUE(y) } static bool empty_y() { return head_y == tail; } static uint16_t free_count_y() { return _free_count_y; } #endif + #if ENABLED(INPUT_SHAPING_Z) + static shaping_time_t peek_z() { return _peek_z; } + static bool dequeue_z() { SHAPING_QUEUE_DEQUEUE(z) } + static bool empty_z() { return head_z == tail; } + static uint16_t free_count_z() { return _free_count_z; } + #endif static void purge() { const auto st = shaping_time_t(-1); #if ENABLED(INPUT_SHAPING_X) - head_x = tail; _free_count_x = shaping_echoes - 1; peek_x_val = st; + head_x = tail; _free_count_x = shaping_echoes - 1; _peek_x = st; #endif #if ENABLED(INPUT_SHAPING_Y) - head_y = tail; _free_count_y = shaping_echoes - 1; peek_y_val = st; + head_y = tail; _free_count_y = shaping_echoes - 1; _peek_y = st; + #endif + #if ENABLED(INPUT_SHAPING_Z) + head_z = tail; _free_count_z = shaping_echoes - 1; _peek_z = st; #endif } }; @@ -420,6 +415,9 @@ class Stepper { #if ENABLED(INPUT_SHAPING_Y) static ShapeParams shaping_y; #endif + #if ENABLED(INPUT_SHAPING_Z) + static ShapeParams shaping_z; + #endif #endif #if ENABLED(LIN_ADVANCE) @@ -517,7 +515,7 @@ class Stepper { const bool was_on = hal.isr_state(); hal.isr_off(); - const bool result = TERN0(INPUT_SHAPING_X, !ShapingQueue::empty_x()) || TERN0(INPUT_SHAPING_Y, !ShapingQueue::empty_y()); + const bool result = TERN0(INPUT_SHAPING_X, !ShapingQueue::empty_x()) || TERN0(INPUT_SHAPING_Y, !ShapingQueue::empty_y()) || TERN0(INPUT_SHAPING_Z, !ShapingQueue::empty_z()); if (was_on) hal.isr_on(); diff --git a/Marlin/src/module/stepper/cycles.h b/Marlin/src/module/stepper/cycles.h index dae9e7c62cd5..0089124eede7 100644 --- a/Marlin/src/module/stepper/cycles.h +++ b/Marlin/src/module/stepper/cycles.h @@ -188,7 +188,7 @@ #define ISR_LOOP_CYCLES(R) ((ISR_LOOP_BASE_CYCLES + MIN_ISR_LOOP_CYCLES + MIN_STEPPER_PULSE_CYCLES) * ((1UL << R) - 1) + _MAX(MIN_ISR_LOOP_CYCLES, MIN_STEPPER_PULSE_CYCLES)) // Model input shaping as an extra loop call -#define ISR_SHAPING_LOOP_CYCLES(R) (TERN0(HAS_ZV_SHAPING, (ISR_LOOP_BASE_CYCLES + TERN0(INPUT_SHAPING_X, ISR_X_STEPPER_CYCLES) + TERN0(INPUT_SHAPING_Y, ISR_Y_STEPPER_CYCLES)) << R)) +#define ISR_SHAPING_LOOP_CYCLES(R) (TERN0(HAS_ZV_SHAPING, (ISR_LOOP_BASE_CYCLES + TERN0(INPUT_SHAPING_X, ISR_X_STEPPER_CYCLES) + TERN0(INPUT_SHAPING_Y, ISR_Y_STEPPER_CYCLES) + TERN0(INPUT_SHAPING_Z, ISR_Z_STEPPER_CYCLES)) << R)) // If linear advance is enabled, then it is handled separately #if ENABLED(LIN_ADVANCE) diff --git a/buildroot/tests/mega2560 b/buildroot/tests/mega2560 index 97d12090c2a0..d61f17b51c6d 100755 --- a/buildroot/tests/mega2560 +++ b/buildroot/tests/mega2560 @@ -96,7 +96,7 @@ opt_set MOTHERBOARD BOARD_AZTEEG_X3_PRO MIXING_STEPPERS 5 LCD_LANGUAGE ru \ opt_enable MIXING_EXTRUDER GRADIENT_MIX GRADIENT_VTOOL CR10_STOCKDISPLAY \ USE_CONTROLLER_FAN CONTROLLER_FAN_EDITABLE CONTROLLER_FAN_IGNORE_Z \ XY_AFTER_HOMING EVENT_GCODE_AFTER_HOMING \ - FILAMENT_RUNOUT_SENSOR ADVANCED_PAUSE_FEATURE NOZZLE_PARK_FEATURE INPUT_SHAPING_X INPUT_SHAPING_Y + FILAMENT_RUNOUT_SENSOR ADVANCED_PAUSE_FEATURE NOZZLE_PARK_FEATURE INPUT_SHAPING_X INPUT_SHAPING_Y INPUT_SHAPING_Z opt_disable DISABLE_OTHER_EXTRUDERS exec_test $1 $2 "Azteeg X3 | Mixing Extruder (x5) | Gradient Mix | Input Shaping | Russian" "$3"