diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 538bbb58a9b5..e3f2c0db7f39 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1057,6 +1057,35 @@ // @section motion +/** + * Input Shaping -- EXPERIMENTAL + * + * Zero Vibration (ZV) Input Shaping for X and/or Y movements. + * + * This option uses a lot of SRAM for the step buffer, which is proportional + * to the largest step rate possible for any axis. If the build fails due to + * low SRAM the buffer size may be reduced by setting smaller values for + * DEFAULT_AXIS_STEPS_PER_UNIT and/or DEFAULT_MAX_FEEDRATE. Runtime editing + * of max feedrate (M203) or resonant frequency (M593) may result feedrate + * being capped to prevent buffer overruns. + * + * Tune with M593 D F: + * + * D Set the zeta/damping factor. If axes (X, Y, etc.) are not specified, set for all axes. + * F Set the frequency. If axes (X, Y, etc.) are not specified, set for all axes. + * T[map] Input Shaping type, 0:ZV, 1:EI, 2:2H EI (not implemented yet) + * X<1> Set the given parameters only for the X axis. + * Y<1> Set the given parameters only for the Y axis. + */ +//#define INPUT_SHAPING +#if ENABLED(INPUT_SHAPING) + #define SHAPING_FREQ_X 40 // (Hz) The dominant resonant frequency of the X axis. + #define SHAPING_FREQ_Y 40 // (Hz) The dominant resonant frequency of the Y axis. + #define SHAPING_ZETA_X 0.3f // Damping ratio of the X axis (range: 0.0 = no damping to 1.0 = critical damping). + #define SHAPING_ZETA_Y 0.3f // Damping ratio of the Y axis (range: 0.0 = no damping to 1.0 = critical damping). + //#define SHAPING_MENU // Add a menu to the LCD to set shaping parameters. +#endif + #define AXIS_RELATIVE_MODES { false, false, false, false } // Add a Duplicate option for well-separated conjoined nozzles diff --git a/Marlin/src/gcode/feature/input_shaping/M593.cpp b/Marlin/src/gcode/feature/input_shaping/M593.cpp new file mode 100644 index 000000000000..84301963cbde --- /dev/null +++ b/Marlin/src/gcode/feature/input_shaping/M593.cpp @@ -0,0 +1,83 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2022 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(INPUT_SHAPING) + +#include "../../gcode.h" +#include "../../../module/stepper.h" + +void GcodeSuite::M593_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F("Input Shaping")); + #if HAS_SHAPING_X + SERIAL_ECHO_MSG("M593 X" + " F", stepper.get_shaping_frequency(X_AXIS), + " D", stepper.get_shaping_damping_ratio(X_AXIS) + ); + #endif + #if HAS_SHAPING_Y + SERIAL_ECHO_MSG("M593 Y" + " F", stepper.get_shaping_frequency(Y_AXIS), + " D", stepper.get_shaping_damping_ratio(Y_AXIS) + ); + #endif +} + +/** + * M593: Get or Set Input Shaping Parameters + * D Set the zeta/damping factor. If axes (X, Y, etc.) are not specified, set for all axes. + * F Set the frequency. If axes (X, Y, etc.) are not specified, set for all axes. + * T[map] Input Shaping type, 0:ZV, 1:EI, 2:2H EI (not implemented yet) + * X<1> Set the given parameters only for the X axis. + * Y<1> Set the given parameters only for the Y axis. + */ +void GcodeSuite::M593() { + if (!parser.seen_any()) return M593_report(); + + const bool seen_X = TERN0(HAS_SHAPING_X, parser.seen_test('X')), + seen_Y = TERN0(HAS_SHAPING_Y, parser.seen_test('Y')), + for_X = seen_X || TERN0(HAS_SHAPING_X, (!seen_X && !seen_Y)), + for_Y = seen_Y || TERN0(HAS_SHAPING_Y, (!seen_X && !seen_Y)); + + 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); + } + else + SERIAL_ECHO_MSG("?Zeta (D) value out of range (0-1)"); + } + + if (parser.seen('F')) { + const float freq = parser.value_float(); + if (freq > 0) { + if (for_X) stepper.set_shaping_frequency(X_AXIS, freq); + if (for_Y) stepper.set_shaping_frequency(Y_AXIS, freq); + } + else + SERIAL_ECHO_MSG("?Frequency (F) must be greater than 0"); + } +} + +#endif diff --git a/Marlin/src/gcode/gcode.cpp b/Marlin/src/gcode/gcode.cpp index 072e943aced4..e14ab5fd6c53 100644 --- a/Marlin/src/gcode/gcode.cpp +++ b/Marlin/src/gcode/gcode.cpp @@ -995,6 +995,10 @@ void GcodeSuite::process_parsed_command(const bool no_ok/*=false*/) { case 575: M575(); break; // M575: Set serial baudrate #endif + #if ENABLED(INPUT_SHAPING) + case 593: M593(); break; // M593: Set Input Shaping parameters + #endif + #if ENABLED(ADVANCED_PAUSE_FEATURE) case 600: M600(); break; // M600: Pause for Filament Change case 603: M603(); break; // M603: Configure Filament Change diff --git a/Marlin/src/gcode/gcode.h b/Marlin/src/gcode/gcode.h index fa53641d2fea..665451f7cd3a 100644 --- a/Marlin/src/gcode/gcode.h +++ b/Marlin/src/gcode/gcode.h @@ -259,6 +259,7 @@ * M554 - Get or set IP gateway. (Requires enabled Ethernet port) * M569 - Enable stealthChop on an axis. (Requires at least one _DRIVER_TYPE to be TMC2130/2160/2208/2209/5130/5160) * M575 - Change the serial baud rate. (Requires BAUD_RATE_GCODE) + * M593 - Get or set input shaping parameters. (Requires INPUT_SHAPING) * M600 - Pause for filament change: "M600 X Y Z E L". (Requires ADVANCED_PAUSE_FEATURE) * M603 - Configure filament change: "M603 T U L". (Requires ADVANCED_PAUSE_FEATURE) * M605 - Set Dual X-Carriage movement mode: "M605 S [X] [R]". (Requires DUAL_X_CARRIAGE) @@ -1085,6 +1086,11 @@ class GcodeSuite { static void M575(); #endif + #if ENABLED(INPUT_SHAPING) + static void M593(); + static void M593_report(const bool forReplay=true); + #endif + #if ENABLED(ADVANCED_PAUSE_FEATURE) static void M600(); static void M603(); diff --git a/Marlin/src/inc/Conditionals_adv.h b/Marlin/src/inc/Conditionals_adv.h index 27185400f951..1f8f0ddfb61e 100644 --- a/Marlin/src/inc/Conditionals_adv.h +++ b/Marlin/src/inc/Conditionals_adv.h @@ -1085,3 +1085,17 @@ #else #define CALC_FAN_SPEED(f) (f ? map(f, 1, 255, FAN_MIN_PWM, FAN_MAX_PWM) : FAN_OFF_PWM) #endif + +// Input shaping +#if ENABLED(INPUT_SHAPING) + #if !HAS_Y_AXIS + #undef SHAPING_FREQ_Y + #undef SHAPING_BUFFER_Y + #endif + #ifdef SHAPING_FREQ_X + #define HAS_SHAPING_X 1 + #endif + #ifdef SHAPING_FREQ_Y + #define HAS_SHAPING_Y 1 + #endif +#endif diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index 679463798a1f..06b8f32946ad 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -4238,11 +4238,6 @@ static_assert(_PLUS_TEST(4), "HOMING_FEEDRATE_MM_M values must be positive."); #endif #endif -// Misc. Cleanup -#undef _TEST_PWM -#undef _NUM_AXES_STR -#undef _LOGICAL_AXES_STR - // JTAG support in the HAL #if ENABLED(DISABLE_DEBUG) && !defined(JTAGSWD_DISABLE) #error "DISABLE_DEBUG is not supported for the selected MCU/Board." @@ -4254,3 +4249,33 @@ static_assert(_PLUS_TEST(4), "HOMING_FEEDRATE_MM_M values must be positive."); #if ENABLED(XFER_BUILD) && !BOTH(BINARY_FILE_TRANSFER, CUSTOM_FIRMWARE_UPLOAD) #error "BINARY_FILE_TRANSFER and CUSTOM_FIRMWARE_UPLOAD are required for custom upload." #endif + +// Check requirements for Input Shaping +#if ENABLED(INPUT_SHAPING) && defined(__AVR__) + #if HAS_SHAPING_X + #if F_CPU > 16000000 + static_assert((SHAPING_FREQ_X) * 2 * 0x10000 >= (STEPPER_TIMER_RATE), "SHAPING_FREQ_X is below the minimum (20) for AVR 20MHz."); + #else + static_assert((SHAPING_FREQ_X) * 2 * 0x10000 >= (STEPPER_TIMER_RATE), "SHAPING_FREQ_X is below the minimum (16) for AVR 16MHz."); + #endif + #elif HAS_SHAPING_Y + #if F_CPU > 16000000 + static_assert((SHAPING_FREQ_Y) * 2 * 0x10000 >= (STEPPER_TIMER_RATE), "SHAPING_FREQ_Y is below the minimum (20) for AVR 20MHz."); + #else + static_assert((SHAPING_FREQ_Y) * 2 * 0x10000 >= (STEPPER_TIMER_RATE), "SHAPING_FREQ_Y is below the minimum (16) for AVR 16MHz."); + #endif + #endif +#endif + +#if ENABLED(INPUT_SHAPING) + #if ENABLED(DIRECT_STEPPING) + #error "INPUT_SHAPING cannot currently be used with DIRECT_STEPPING." + #elif ENABLED(LASER_FEATURE) + #error "INPUT_SHAPING cannot currently be used with LASER_FEATURE." + #endif +#endif + +// Misc. Cleanup +#undef _TEST_PWM +#undef _NUM_AXES_STR +#undef _LOGICAL_AXES_STR diff --git a/Marlin/src/lcd/language/language_en.h b/Marlin/src/lcd/language/language_en.h index 8933d4fa458d..0aeb171de083 100644 --- a/Marlin/src/lcd/language/language_en.h +++ b/Marlin/src/lcd/language/language_en.h @@ -400,6 +400,11 @@ namespace Language_en { LSTR MSG_AMAX_EN = _UxGT("Max * Accel"); LSTR MSG_A_RETRACT = _UxGT("Retract Accel"); LSTR MSG_A_TRAVEL = _UxGT("Travel Accel"); + LSTR MSG_INPUT_SHAPING = _UxGT("Input Shaping"); + LSTR MSG_SHAPING_X_FREQ = STR_X _UxGT(" frequency"); + LSTR MSG_SHAPING_Y_FREQ = STR_Y _UxGT(" frequency"); + LSTR MSG_SHAPING_X_ZETA = STR_X _UxGT(" damping"); + LSTR MSG_SHAPING_Y_ZETA = STR_Y _UxGT(" damping"); LSTR MSG_XY_FREQUENCY_LIMIT = _UxGT("XY Freq Limit"); LSTR MSG_XY_FREQUENCY_FEEDRATE = _UxGT("Min FR Factor"); LSTR MSG_STEPS_PER_MM = _UxGT("Steps/mm"); diff --git a/Marlin/src/lcd/menu/menu_advanced.cpp b/Marlin/src/lcd/menu/menu_advanced.cpp index 2037db18b557..ec55259d2567 100644 --- a/Marlin/src/lcd/menu/menu_advanced.cpp +++ b/Marlin/src/lcd/menu/menu_advanced.cpp @@ -96,8 +96,6 @@ void menu_bedlevel_points(); #if HAS_MOTOR_CURRENT_PWM - #include "../../module/stepper.h" - void menu_pwm() { START_MENU(); //BACK_ITEM(MSG_ADVANCED_SETTINGS); @@ -661,6 +659,39 @@ void menu_bedlevel_points(); END_MENU(); } + #if ENABLED(SHAPING_MENU) + + void menu_advanced_input_shaping() { + constexpr float min_frequency = TERN(__AVR__, float(STEPPER_TIMER_RATE) / 2 / 0x10000, 1.0f); + + START_MENU(); + BACK_ITEM(MSG_ADVANCED_SETTINGS); + + // M593 F Frequency + #if HAS_SHAPING_X + editable.decimal = stepper.get_shaping_frequency(X_AXIS); + EDIT_ITEM_FAST(float61, MSG_SHAPING_X_FREQ, &editable.decimal, min_frequency, 200.0f, []{ stepper.set_shaping_frequency(X_AXIS, editable.decimal); }); + #endif + #if HAS_SHAPING_Y + editable.decimal = stepper.get_shaping_frequency(Y_AXIS); + EDIT_ITEM_FAST(float61, MSG_SHAPING_Y_FREQ, &editable.decimal, min_frequency, 200.0f, []{ stepper.set_shaping_frequency(Y_AXIS, editable.decimal); }); + #endif + + // M593 D Damping ratio + #if HAS_SHAPING_X + editable.decimal = stepper.get_shaping_damping_ratio(X_AXIS); + EDIT_ITEM_FAST(float42_52, MSG_SHAPING_X_ZETA, &editable.decimal, 0.0f, 1.0f, []{ stepper.set_shaping_damping_ratio(X_AXIS, editable.decimal); }); + #endif + #if HAS_SHAPING_Y + editable.decimal = stepper.get_shaping_damping_ratio(Y_AXIS); + EDIT_ITEM_FAST(float42_52, MSG_SHAPING_Y_ZETA, &editable.decimal, 0.0f, 1.0f, []{ stepper.set_shaping_damping_ratio(Y_AXIS, editable.decimal); }); + #endif + + END_MENU(); + } + + #endif + #if HAS_CLASSIC_JERK void menu_advanced_jerk() { @@ -814,6 +845,11 @@ void menu_advanced_settings() { // M201 - Acceleration items SUBMENU(MSG_ACCELERATION, menu_advanced_acceleration); + // M593 - Acceleration items + #if ENABLED(SHAPING_MENU) + SUBMENU(MSG_INPUT_SHAPING, menu_advanced_input_shaping); + #endif + #if HAS_CLASSIC_JERK // M205 - Max Jerk SUBMENU(MSG_JERK, menu_advanced_jerk); diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index cfbfe145b2e1..cfe381ec5fcd 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -2488,6 +2488,14 @@ bool Planner::_populate_block( #endif // XY_FREQUENCY_LIMIT + #if ENABLED(INPUT_SHAPING) + const float top_freq = _MIN(float(0x7FFFFFFFL) + OPTARG(HAS_SHAPING_X, stepper.get_shaping_frequency(X_AXIS)) + OPTARG(HAS_SHAPING_Y, stepper.get_shaping_frequency(Y_AXIS))), + max_factor = (top_freq * float(shaping_dividends - 3) * 2.0f) / block->nominal_rate; + NOMORE(speed_factor, max_factor); + #endif + // Correct the speed if (speed_factor < 1.0f) { current_speed *= speed_factor; diff --git a/Marlin/src/module/settings.cpp b/Marlin/src/module/settings.cpp index 6f4f2f02db88..93a9d7ace5e3 100644 --- a/Marlin/src/module/settings.cpp +++ b/Marlin/src/module/settings.cpp @@ -582,6 +582,18 @@ typedef struct SettingsDataStruct { MPC_t mpc_constants[HOTENDS]; // M306 #endif + // + // Input Shaping + // + #if HAS_SHAPING_X + float shaping_x_frequency, // M593 X F + shaping_x_zeta; // M593 X D + #endif + #if HAS_SHAPING_Y + float shaping_y_frequency, // M593 Y F + shaping_y_zeta; // M593 Y D + #endif + planner_axinvert_t invert_axes; thermistors_data_t thermistors_data; celsius_t hotend_maxtemp[HOTENDS]; @@ -1640,6 +1652,20 @@ void MarlinSettings::postprocess() { EEPROM_WRITE(thermalManager.temp_hotend[e].constants); #endif + // + // Input Shaping + /// + #if ENABLED(INPUT_SHAPING) + #if HAS_SHAPING_X + EEPROM_WRITE(stepper.get_shaping_frequency(X_AXIS)); + EEPROM_WRITE(stepper.get_shaping_damping_ratio(X_AXIS)); + #endif + #if HAS_SHAPING_Y + EEPROM_WRITE(stepper.get_shaping_frequency(Y_AXIS)); + EEPROM_WRITE(stepper.get_shaping_damping_ratio(Y_AXIS)); + #endif + #endif + // // Report final CRC and Data Size // @@ -2644,6 +2670,27 @@ void MarlinSettings::postprocess() { } #endif + // + // Input Shaping + // + #if HAS_SHAPING_X + { + float _data[2]; + EEPROM_READ(_data); + stepper.set_shaping_frequency(X_AXIS, _data[0]); + stepper.set_shaping_damping_ratio(X_AXIS, _data[1]); + } + #endif + + #if HAS_SHAPING_Y + { + float _data[2]; + EEPROM_READ(_data); + stepper.set_shaping_frequency(Y_AXIS, _data[0]); + stepper.set_shaping_damping_ratio(Y_AXIS, _data[1]); + } + #endif + // // Validate Final Size and CRC // @@ -3520,6 +3567,20 @@ void MarlinSettings::reset() { } #endif + // + // Input Shaping + // + #if ENABLED(INPUT_SHAPING) + #if HAS_SHAPING_X + stepper.set_shaping_frequency(X_AXIS, SHAPING_FREQ_X); + stepper.set_shaping_damping_ratio(X_AXIS, SHAPING_ZETA_X); + #endif + #if HAS_SHAPING_Y + stepper.set_shaping_frequency(Y_AXIS, SHAPING_FREQ_Y); + stepper.set_shaping_damping_ratio(Y_AXIS, SHAPING_ZETA_Y); + #endif + #endif + postprocess(); #if EITHER(EEPROM_CHITCHAT, DEBUG_LEVELING_FEATURE) @@ -3770,6 +3831,11 @@ void MarlinSettings::reset() { // TERN_(HAS_STEALTHCHOP, gcode.M569_report(forReplay)); + // + // Input Shaping + // + TERN_(INPUT_SHAPING, gcode.M593_report(forReplay)); + // // Linear Advance // diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 0c1e28a00cc2..6b69bad66c53 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -199,7 +199,7 @@ IF_DISABLED(ADAPTIVE_STEP_SMOOTHING, constexpr) uint8_t Stepper::oversampling_fa xyze_long_t Stepper::delta_error{0}; -xyze_ulong_t Stepper::advance_dividend{0}; +xyze_long_t Stepper::advance_dividend{0}; uint32_t Stepper::advance_divisor = 0, Stepper::step_events_completed = 0, // The number of step events executed in the current block Stepper::accelerate_until, // The count at which to stop accelerating @@ -232,6 +232,20 @@ uint32_t Stepper::advance_divisor = 0, Stepper::la_advance_steps = 0; #endif +#if ENABLED(INPUT_SHAPING) + shaping_time_t DelayTimeManager::now = 0; + ParamDelayQueue Stepper::shaping_dividend_queue; + DelayQueue Stepper::shaping_queue; + #if HAS_SHAPING_X + shaping_time_t DelayTimeManager::delay_x; + ShapeParams Stepper::shaping_x; + #endif + #if HAS_SHAPING_Y + shaping_time_t DelayTimeManager::delay_y; + ShapeParams Stepper::shaping_y; + #endif +#endif + #if ENABLED(INTEGRATED_BABYSTEPPING) uint32_t Stepper::nextBabystepISR = BABYSTEP_NEVER; #endif @@ -458,12 +472,10 @@ xyze_int8_t Stepper::count_direction{0}; #define PULSE_LOW_TICK_COUNT hal_timer_t(NS_TO_PULSE_TIMER_TICKS(_MIN_PULSE_LOW_NS - _MIN(_MIN_PULSE_LOW_NS, TIMER_SETUP_NS))) #define USING_TIMED_PULSE() hal_timer_t start_pulse_count = 0 -#define START_TIMED_PULSE(DIR) (start_pulse_count = HAL_timer_get_count(MF_TIMER_PULSE)) -#define AWAIT_TIMED_PULSE(DIR) while (PULSE_##DIR##_TICK_COUNT > HAL_timer_get_count(MF_TIMER_PULSE) - start_pulse_count) { } -#define START_HIGH_PULSE() START_TIMED_PULSE(HIGH) -#define AWAIT_HIGH_PULSE() AWAIT_TIMED_PULSE(HIGH) -#define START_LOW_PULSE() START_TIMED_PULSE(LOW) -#define AWAIT_LOW_PULSE() AWAIT_TIMED_PULSE(LOW) +#define START_TIMED_PULSE() (start_pulse_count = HAL_timer_get_count(MF_TIMER_PULSE)) +#define AWAIT_TIMED_PULSE(DIR) while (PULSE_##DIR##_TICK_COUNT > HAL_timer_get_count(MF_TIMER_PULSE) - start_pulse_count) { /* nada */ } +#define AWAIT_HIGH_PULSE() AWAIT_TIMED_PULSE(HIGH) +#define AWAIT_LOW_PULSE() AWAIT_TIMED_PULSE(LOW) #if MINIMUM_STEPPER_PRE_DIR_DELAY > 0 #define DIR_WAIT_BEFORE() DELAY_NS(MINIMUM_STEPPER_PRE_DIR_DELAY) @@ -559,6 +571,16 @@ void Stepper::disable_all_steppers() { TERN_(EXTENSIBLE_UI, ExtUI::onSteppersDisabled()); } +#define SET_STEP_DIR(A) \ + if (motor_direction(_AXIS(A))) { \ + A##_APPLY_DIR(INVERT_##A##_DIR, false); \ + count_direction[_AXIS(A)] = -1; \ + } \ + else { \ + A##_APPLY_DIR(!INVERT_##A##_DIR, false); \ + count_direction[_AXIS(A)] = 1; \ + } + /** * Set the stepper direction of each axis * @@ -1466,8 +1488,20 @@ void Stepper::isr() { // Enable ISRs to reduce USART processing latency hal.isr_on(); + #if ENABLED(INPUT_SHAPING) + // Speed limiting should ensure the buffers never get full. But if somehow they do, stutter rather than overflow. + if (!nextMainISR) { + TERN_(HAS_SHAPING_X, if (shaping_dividend_queue.free_count_x() == 0) nextMainISR = shaping_dividend_queue.peek_x() + 1); + TERN_(HAS_SHAPING_Y, if (shaping_dividend_queue.free_count_y() == 0) NOLESS(nextMainISR, shaping_dividend_queue.peek_y() + 1)); + TERN_(HAS_SHAPING_X, if (shaping_queue.free_count_x() < steps_per_isr) NOLESS(nextMainISR, shaping_queue.peek_x() + 1)); + TERN_(HAS_SHAPING_Y, if (shaping_queue.free_count_y() < steps_per_isr) NOLESS(nextMainISR, shaping_queue.peek_y() + 1)); + } + #endif + if (!nextMainISR) pulse_phase_isr(); // 0 = Do coordinated axes Stepper pulses + TERN_(INPUT_SHAPING, shaping_isr()); // Do Shaper stepping, if needed + #if ENABLED(LIN_ADVANCE) if (!nextAdvanceISR) { // 0 = Do Linear Advance E Stepper pulses advance_isr(); @@ -1496,10 +1530,14 @@ void Stepper::isr() { // Get the interval to the next ISR call const uint32_t interval = _MIN( - uint32_t(HAL_TIMER_TYPE_MAX), // Come back in a very long time - nextMainISR // Time until the next Pulse / Block phase - OPTARG(LIN_ADVANCE, nextAdvanceISR) // Come back early for Linear Advance? - OPTARG(INTEGRATED_BABYSTEPPING, nextBabystepISR) // Come back early for Babystepping? + uint32_t(HAL_TIMER_TYPE_MAX), // Come back in a very long time + nextMainISR // Time until the next Pulse / Block phase + OPTARG(HAS_SHAPING_X, shaping_dividend_queue.peek_x()) // Time until next input shaping dividend change for X + OPTARG(HAS_SHAPING_Y, shaping_dividend_queue.peek_y()) // Time until next input shaping dividend change for Y + OPTARG(HAS_SHAPING_X, shaping_queue.peek_x()) // Time until next input shaping echo for X + OPTARG(HAS_SHAPING_Y, shaping_queue.peek_y()) // Time until next input shaping echo for Y + OPTARG(LIN_ADVANCE, nextAdvanceISR) // Come back early for Linear Advance? + OPTARG(INTEGRATED_BABYSTEPPING, nextBabystepISR) // Come back early for Babystepping? ); // @@ -1511,6 +1549,8 @@ void Stepper::isr() { nextMainISR -= interval; + TERN_(INPUT_SHAPING, DelayTimeManager::decrement_delays(interval)); + #if ENABLED(LIN_ADVANCE) if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval; #endif @@ -1603,11 +1643,19 @@ 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 (current_block) { + discard_current_block(); + #if ENABLED(INPUT_SHAPING) + shaping_dividend_queue.purge(); + shaping_queue.purge(); + TERN_(HAS_SHAPING_X, delta_error.x = 0); + TERN_(HAS_SHAPING_Y, delta_error.y = 0); + #endif + } } // If there is no current block, do nothing - if (!current_block) return; + if (!current_block || step_events_completed >= step_event_count) return; // Skipping step processing causes motion to freeze if (TERN0(FREEZE_FEATURE, frozen)) return; @@ -1626,6 +1674,9 @@ void Stepper::pulse_phase_isr() { #endif xyze_bool_t step_needed{0}; + // Direct Stepping page? + const bool is_page = current_block->is_page(); + do { #define _APPLY_STEP(AXIS, INV, ALWAYS) AXIS ##_APPLY_STEP(INV, ALWAYS) #define _INVERT_STEP_PIN(AXIS) INVERT_## AXIS ##_STEP_PIN @@ -1640,6 +1691,22 @@ void Stepper::pulse_phase_isr() { } \ }while(0) + #define PULSE_PREP_SHAPING(AXIS, DIVIDEND) do{ \ + delta_error[_AXIS(AXIS)] += (DIVIDEND); \ + if ((MAXDIR(AXIS) && delta_error[_AXIS(AXIS)] <= -0x30000000L) || (MINDIR(AXIS) && delta_error[_AXIS(AXIS)] >= 0x30000000L)) { \ + TBI(last_direction_bits, _AXIS(AXIS)); \ + DIR_WAIT_BEFORE(); \ + SET_STEP_DIR(AXIS); \ + DIR_WAIT_AFTER(); \ + } \ + step_needed[_AXIS(AXIS)] = (MAXDIR(AXIS) && delta_error[_AXIS(AXIS)] >= 0x10000000L) || \ + (MINDIR(AXIS) && delta_error[_AXIS(AXIS)] <= -0x10000000L); \ + if (step_needed[_AXIS(AXIS)]) { \ + count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \ + delta_error[_AXIS(AXIS)] += MAXDIR(AXIS) ? -0x20000000L : 0x20000000L; \ + } \ + }while(0) + // Start an active pulse if needed #define PULSE_START(AXIS) do{ \ if (step_needed[_AXIS(AXIS)]) { \ @@ -1654,9 +1721,6 @@ void Stepper::pulse_phase_isr() { } \ }while(0) - // Direct Stepping page? - const bool is_page = current_block->is_page(); - #if ENABLED(DIRECT_STEPPING) // Direct stepping is currently not ready for HAS_I_AXIS if (is_page) { @@ -1764,12 +1828,22 @@ void Stepper::pulse_phase_isr() { #endif // DIRECT_STEPPING if (!is_page) { + TERN_(INPUT_SHAPING, shaping_queue.enqueue()); + // Determine if pulses are needed #if HAS_X_STEP - PULSE_PREP(X); + #if HAS_SHAPING_X + PULSE_PREP_SHAPING(X, advance_dividend.x); + #else + PULSE_PREP(X); + #endif #endif #if HAS_Y_STEP - PULSE_PREP(Y); + #if HAS_SHAPING_Y + PULSE_PREP_SHAPING(Y, advance_dividend.y); + #else + PULSE_PREP(Y); + #endif #endif #if HAS_Z_STEP PULSE_PREP(Z); @@ -1854,7 +1928,7 @@ void Stepper::pulse_phase_isr() { // TODO: need to deal with MINIMUM_STEPPER_PULSE over i2s #if ISR_MULTI_STEPS - START_HIGH_PULSE(); + START_TIMED_PULSE(); AWAIT_HIGH_PULSE(); #endif @@ -1894,12 +1968,62 @@ void Stepper::pulse_phase_isr() { #endif #if ISR_MULTI_STEPS - if (events_to_do) START_LOW_PULSE(); + if (events_to_do) START_TIMED_PULSE(); #endif } while (--events_to_do); } +#if ENABLED(INPUT_SHAPING) + + void Stepper::shaping_isr() { + xyze_bool_t step_needed{0}; + + const bool shapex = TERN0(HAS_SHAPING_X, !shaping_queue.peek_x()), + shapey = TERN0(HAS_SHAPING_Y, !shaping_queue.peek_y()); + + #if HAS_SHAPING_X + if (!shaping_dividend_queue.peek_x()) shaping_x.dividend = shaping_dividend_queue.dequeue_x(); + #endif + #if HAS_SHAPING_Y + if (!shaping_dividend_queue.peek_y()) shaping_y.dividend = shaping_dividend_queue.dequeue_y(); + #endif + + #if HAS_SHAPING_X + if (shapex) { + shaping_queue.dequeue_x(); + PULSE_PREP_SHAPING(X, shaping_x.dividend); + PULSE_START(X); + } + #endif + + #if HAS_SHAPING_Y + if (shapey) { + shaping_queue.dequeue_y(); + PULSE_PREP_SHAPING(Y, shaping_y.dividend); + PULSE_START(Y); + } + #endif + + TERN_(I2S_STEPPER_STREAM, i2s_push_sample()); + + if (shapex || shapey) { + #if ISR_MULTI_STEPS + USING_TIMED_PULSE(); + START_TIMED_PULSE(); + AWAIT_HIGH_PULSE(); + #endif + #if HAS_SHAPING_X + if (shapex) PULSE_STOP(X); + #endif + #if HAS_SHAPING_Y + if (shapey) PULSE_STOP(Y); + #endif + } + } + +#endif // INPUT_SHAPING + // Calculate timer interval, with all limits applied. uint32_t Stepper::calc_timer_interval(uint32_t step_rate) { #ifdef CPU_32_BIT @@ -2362,12 +2486,56 @@ uint32_t Stepper::block_phase_isr() { step_event_count = current_block->step_event_count << oversampling; // Initialize Bresenham delta errors to 1/2 + #if HAS_SHAPING_X + const int32_t old_delta_error_x = delta_error.x; + #endif + #if HAS_SHAPING_Y + const int32_t old_delta_error_y = delta_error.y; + #endif delta_error = TERN_(LIN_ADVANCE, la_delta_error =) -int32_t(step_event_count); // Calculate Bresenham dividends and divisors - advance_dividend = current_block->steps << 1; + advance_dividend = (current_block->steps << 1).asLong(); advance_divisor = step_event_count << 1; + // for input shaped axes, advance_divisor is replaced with 0x40000000 + // and steps are repeated twice so dividends have to be scaled and halved + // and the dividend is directional, i.e. signed + TERN_(HAS_SHAPING_X, advance_dividend.x = (uint64_t(current_block->steps.x) << 29) / step_event_count); + TERN_(HAS_SHAPING_X, if (TEST(current_block->direction_bits, X_AXIS)) advance_dividend.x *= -1); + TERN_(HAS_SHAPING_X, if (!shaping_queue.empty_x()) SET_BIT_TO(current_block->direction_bits, X_AXIS, TEST(last_direction_bits, X_AXIS))); + TERN_(HAS_SHAPING_Y, advance_dividend.y = (uint64_t(current_block->steps.y) << 29) / step_event_count); + TERN_(HAS_SHAPING_Y, if (TEST(current_block->direction_bits, Y_AXIS)) advance_dividend.y *= -1); + TERN_(HAS_SHAPING_Y, if (!shaping_queue.empty_y()) SET_BIT_TO(current_block->direction_bits, Y_AXIS, TEST(last_direction_bits, Y_AXIS))); + + // The scaling operation above introduces rounding errors which must now be removed. + // For this segment, there will be step_event_count calls to the Bresenham logic and the same number of echoes. + // For each pair of calls to the Bresenham logic, delta_error will increase by advance_dividend modulo 0x20000000 + // so (e.g. for x) delta_error.x will end up changing by (advance_dividend.x * step_event_count) % 0x20000000. + // For a divisor which is a power of 2, modulo is the same as as a bitmask, i.e. + // (advance_dividend.x * step_event_count) & 0x1FFFFFFF. + // This segment's final change in delta_error should actually be zero so we need to increase delta_error by + // 0 - ((advance_dividend.x * step_event_count) & 0x1FFFFFFF) + // And this needs to be adjusted to the range -0x10000000 to 0x10000000. + // Adding and subtracting 0x10000000 inside the outside the modulo achieves this. + TERN_(HAS_SHAPING_X, delta_error.x = old_delta_error_x + 0x10000000L - ((0x10000000L + advance_dividend.x * step_event_count) & 0x1FFFFFFFUL)); + TERN_(HAS_SHAPING_Y, delta_error.y = old_delta_error_y + 0x10000000L - ((0x10000000L + advance_dividend.y * step_event_count) & 0x1FFFFFFFUL)); + + // when there is damping, the signal and its echo have different amplitudes + #if ENABLED(HAS_SHAPING_X) + const int32_t echo_x = shaping_x.factor * (advance_dividend.x >> 7); + #endif + #if ENABLED(HAS_SHAPING_Y) + const int32_t echo_y = shaping_y.factor * (advance_dividend.y >> 7); + #endif + + // plan the change of values for advance_dividend for the input shaping echoes + TERN_(INPUT_SHAPING, shaping_dividend_queue.enqueue(TERN0(HAS_SHAPING_X, echo_x), TERN0(HAS_SHAPING_Y, echo_y))); + + // apply the adjustment to the primary signal + TERN_(HAS_SHAPING_X, advance_dividend.x -= echo_x); + TERN_(HAS_SHAPING_Y, advance_dividend.y -= echo_y); + // No step events completed so far step_events_completed = 0; @@ -2482,7 +2650,7 @@ uint32_t Stepper::block_phase_isr() { // Enforce a minimum duration for STEP pulse ON #if ISR_PULSE_CONTROL USING_TIMED_PULSE(); - START_HIGH_PULSE(); + START_TIMED_PULSE(); AWAIT_HIGH_PULSE(); #endif @@ -2814,6 +2982,51 @@ void Stepper::init() { #endif } +#if ENABLED(INPUT_SHAPING) + /** + * Calculate a fixed point factor to apply to the signal and its echo + * when shaping an axis. + */ + void Stepper::set_shaping_damping_ratio(const AxisEnum axis, const float zeta) { + // from the damping ratio, get a factor that can be applied to advance_dividend for fixed point maths + // for ZV, we use amplitudes 1/(1+K) and K/(1+K) where K = exp(-zeta * M_PI / sqrt(1.0f - zeta * zeta)) + // which can be converted to 1:7 fixed point with an excellent fit with a 3rd order polynomial + float shaping_factor; + if (zeta <= 0.0f) shaping_factor = 64.0f; + else if (zeta >= 1.0f) shaping_factor = 0.0f; + else { + shaping_factor = 64.44056192 + -99.02008832 * zeta; + const float zeta2 = zeta * zeta; + shaping_factor += -7.58095488 * zeta2; + const float zeta3 = zeta2 * zeta; + shaping_factor += 43.073216 * zeta3; + } + + const bool was_on = hal.isr_state(); + hal.isr_off(); + TERN_(HAS_SHAPING_X, if (axis == X_AXIS) { shaping_x.factor = floor(shaping_factor); shaping_x.zeta = zeta; }) + TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) { shaping_y.factor = floor(shaping_factor); shaping_y.zeta = zeta; }) + if (was_on) hal.isr_on(); + } + + float Stepper::get_shaping_damping_ratio(const AxisEnum axis) { + TERN_(HAS_SHAPING_X, if (axis == X_AXIS) return shaping_x.zeta); + TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) return shaping_y.zeta); + return -1; + } + + void Stepper::set_shaping_frequency(const AxisEnum axis, const float freq) { + TERN_(HAS_SHAPING_X, if (axis == X_AXIS) { DelayTimeManager::set_delay(axis, float(uint32_t(STEPPER_TIMER_RATE) / 2) / freq); shaping_x.frequency = freq; }) + TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) { DelayTimeManager::set_delay(axis, float(uint32_t(STEPPER_TIMER_RATE) / 2) / freq); shaping_y.frequency = freq; }) + } + + float Stepper::get_shaping_frequency(const AxisEnum axis) { + TERN_(HAS_SHAPING_X, if (axis == X_AXIS) return shaping_x.frequency); + TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) return shaping_y.frequency); + return -1; + } +#endif + /** * Set the stepper positions directly in steps * @@ -3019,7 +3232,7 @@ void Stepper::report_positions() { #if EXTRA_CYCLES_BABYSTEP > 20 #define _SAVE_START() const hal_timer_t pulse_start = HAL_timer_get_count(MF_TIMER_PULSE) - #define _PULSE_WAIT() while (EXTRA_CYCLES_BABYSTEP > (uint32_t)(HAL_timer_get_count(MF_TIMER_PULSE) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ } + #define _PULSE_WAIT() while (EXTRA_CYCLES_BABYSTEP > uint32_t(HAL_timer_get_count(MF_TIMER_PULSE) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ } #else #define _SAVE_START() NOOP #if EXTRA_CYCLES_BABYSTEP > 0 diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 537f6281794c..cf22914541e6 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -312,6 +312,117 @@ constexpr ena_mask_t enable_overlap[] = { //static_assert(!any_enable_overlap(), "There is some overlap."); +#if ENABLED(INPUT_SHAPING) + + typedef IF::type shaping_time_t; + + // These constexpr are used to calculate the shaping queue buffer sizes + constexpr xyze_float_t max_feedrate = DEFAULT_MAX_FEEDRATE; + constexpr xyze_float_t steps_per_unit = DEFAULT_AXIS_STEPS_PER_UNIT; + constexpr float max_steprate = _MAX(LOGICAL_AXIS_LIST( + max_feedrate.e * steps_per_unit.e, + max_feedrate.x * steps_per_unit.x, + max_feedrate.y * steps_per_unit.y, + max_feedrate.z * steps_per_unit.z, + max_feedrate.i * steps_per_unit.i, + max_feedrate.j * steps_per_unit.j, + max_feedrate.k * steps_per_unit.k, + max_feedrate.u * steps_per_unit.u, + max_feedrate.v * steps_per_unit.v, + max_feedrate.w * steps_per_unit.w + )); + constexpr uint16_t shaping_dividends = max_steprate / _MIN(0x7FFFFFFFL OPTARG(HAS_SHAPING_X, SHAPING_FREQ_X) OPTARG(HAS_SHAPING_Y, SHAPING_FREQ_Y)) / 2 + 3; + constexpr uint16_t shaping_segments = max_steprate / (MIN_STEPS_PER_SEGMENT) / _MIN(0x7FFFFFFFL OPTARG(HAS_SHAPING_X, SHAPING_FREQ_X) OPTARG(HAS_SHAPING_Y, SHAPING_FREQ_Y)) / 2 + 3; + + class DelayTimeManager { + private: + static shaping_time_t now; + #ifdef HAS_SHAPING_X + static shaping_time_t delay_x; + #endif + #ifdef HAS_SHAPING_Y + static shaping_time_t delay_y; + #endif + public: + static void decrement_delays(const shaping_time_t interval) { now += interval; } + static void set_delay(const AxisEnum axis, const shaping_time_t delay) { + TERN_(HAS_SHAPING_X, if (axis == X_AXIS) delay_x = delay); + TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) delay_y = delay); + } + }; + + template + class DelayQueue : public DelayTimeManager { + protected: + shaping_time_t times[SIZE]; + uint16_t tail = 0 OPTARG(HAS_SHAPING_X, head_x = 0) OPTARG(HAS_SHAPING_Y, head_y = 0); + + public: + void enqueue() { + times[tail] = now; + if (++tail == SIZE) tail = 0; + } + #ifdef HAS_SHAPING_X + shaping_time_t peek_x() { + if (head_x != tail) return times[head_x] + delay_x - now; + else return shaping_time_t(-1); + } + void dequeue_x() { if (++head_x == SIZE) head_x = 0; } + bool empty_x() { return head_x == tail; } + uint16_t free_count_x() { return head_x > tail ? head_x - tail - 1 : head_x + SIZE - tail - 1; } + #endif + #ifdef HAS_SHAPING_Y + shaping_time_t peek_y() { + if (head_y != tail) return times[head_y] + delay_y - now; + else return shaping_time_t(-1); + } + void dequeue_y() { if (++head_y == SIZE) head_y = 0; } + bool empty_y() { return head_y == tail; } + uint16_t free_count_y() { return head_y > tail ? head_y - tail - 1 : head_y + SIZE - tail - 1; } + #endif + void purge() { auto temp = TERN_(HAS_SHAPING_X, head_x) = TERN_(HAS_SHAPING_Y, head_y) = tail; UNUSED(temp);} + }; + + class ParamDelayQueue : public DelayQueue { + private: + #ifdef HAS_SHAPING_X + int32_t params_x[shaping_segments]; + #endif + #ifdef HAS_SHAPING_Y + int32_t params_y[shaping_segments]; + #endif + + public: + void enqueue(const int32_t param_x, const int32_t param_y) { + TERN(HAS_SHAPING_X, params_x[DelayQueue::tail] = param_x, UNUSED(param_x)); + TERN(HAS_SHAPING_Y, params_y[DelayQueue::tail] = param_y, UNUSED(param_y)); + DelayQueue::enqueue(); + } + #ifdef HAS_SHAPING_X + const int32_t dequeue_x() { + const int32_t result = params_x[DelayQueue::head_x]; + DelayQueue::dequeue_x(); + return result; + } + #endif + #ifdef HAS_SHAPING_Y + const int32_t dequeue_y() { + const int32_t result = params_y[DelayQueue::head_y]; + DelayQueue::dequeue_y(); + return result; + } + #endif + }; + + struct ShapeParams { + float frequency; + float zeta; + uint8_t factor; + int32_t dividend; + }; + +#endif // INPUT_SHAPING + // // Stepper class definition // @@ -391,7 +502,7 @@ class Stepper { // Delta error variables for the Bresenham line tracer static xyze_long_t delta_error; - static xyze_ulong_t advance_dividend; + static xyze_long_t advance_dividend; static uint32_t advance_divisor, step_events_completed, // The number of step events executed in the current block accelerate_until, // The point from where we need to stop acceleration @@ -416,6 +527,17 @@ class Stepper { static bool bezier_2nd_half; // If Bézier curve has been initialized or not #endif + #if ENABLED(INPUT_SHAPING) + static ParamDelayQueue shaping_dividend_queue; + static DelayQueue shaping_queue; + #if HAS_SHAPING_X + static ShapeParams shaping_x; + #endif + #if HAS_SHAPING_Y + static ShapeParams shaping_y; + #endif + #endif + #if ENABLED(LIN_ADVANCE) static constexpr uint32_t LA_ADV_NEVER = 0xFFFFFFFF; static uint32_t nextAdvanceISR, @@ -475,6 +597,10 @@ class Stepper { // The stepper block processing ISR phase static uint32_t block_phase_isr(); + #if ENABLED(INPUT_SHAPING) + static void shaping_isr(); + #endif + #if ENABLED(LIN_ADVANCE) // The Linear advance ISR phase static void advance_isr(); @@ -628,6 +754,13 @@ class Stepper { set_directions(); } + #if ENABLED(INPUT_SHAPING) + static void set_shaping_damping_ratio(const AxisEnum axis, const float zeta); + static float get_shaping_damping_ratio(const AxisEnum axis); + static void set_shaping_frequency(const AxisEnum axis, const float freq); + static float get_shaping_frequency(const AxisEnum axis); + #endif + private: // Set the current position in steps diff --git a/buildroot/tests/mega2560 b/buildroot/tests/mega2560 index 86b27790324d..333be2f0faae 100755 --- a/buildroot/tests/mega2560 +++ b/buildroot/tests/mega2560 @@ -80,7 +80,7 @@ opt_set MOTHERBOARD BOARD_AZTEEG_X3_PRO MIXING_STEPPERS 5 LCD_LANGUAGE ru \ FIL_RUNOUT2_PIN 16 FIL_RUNOUT3_PIN 17 FIL_RUNOUT4_PIN 4 FIL_RUNOUT5_PIN 5 opt_enable MIXING_EXTRUDER GRADIENT_MIX GRADIENT_VTOOL CR10_STOCKDISPLAY \ USE_CONTROLLER_FAN CONTROLLER_FAN_EDITABLE CONTROLLER_FAN_IGNORE_Z \ - FILAMENT_RUNOUT_SENSOR ADVANCED_PAUSE_FEATURE NOZZLE_PARK_FEATURE + FILAMENT_RUNOUT_SENSOR ADVANCED_PAUSE_FEATURE NOZZLE_PARK_FEATURE INPUT_SHAPING opt_disable DISABLE_INACTIVE_EXTRUDER exec_test $1 $2 "Azteeg X3 | Mixing Extruder (x5) | Gradient Mix | Greek" "$3" diff --git a/ini/features.ini b/ini/features.ini index 61ec5ad34d8c..9208fa1343ab 100644 --- a/ini/features.ini +++ b/ini/features.ini @@ -187,6 +187,7 @@ HAS_DUPLICATION_MODE = src_filter=+ PHOTO_GCODE = src_filter=+ CONTROLLER_FAN_EDITABLE = src_filter=+ +INPUT_SHAPING = src_filter=+ GCODE_MACROS = src_filter=+ GRADIENT_MIX = src_filter=+ HAS_SAVED_POSITIONS = src_filter=+ + diff --git a/platformio.ini b/platformio.ini index d661322a8386..3c963e6fa597 100644 --- a/platformio.ini +++ b/platformio.ini @@ -191,6 +191,7 @@ default_src_filter = + - - + - - - + - - - -