MPCNC/Firmware2
1
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

⚡️ Input Shaping improvements (#24951)

Browse Source
This commit is contained in:
tombrazier 2022-11-28 03:38:15 +00:00 committed by Scott Lahteine
parent d2ece1e713
commit 57f6c93192
15 changed files with 468 additions and 367 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
Marlin/Configuration_adv.h
View File

@ -1062,12 +1062,14 @@
* *
* Zero Vibration (ZV) Input Shaping for X and/or Y movements. * 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 * This option uses a lot of SRAM for the step buffer, which is related to the
* to the largest step rate possible for any axis. If the build fails due to * largest step rate possible for the shaped axes. If the build fails due to
* low SRAM the buffer size may be reduced by setting smaller values for * 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 * DEFAULT_AXIS_STEPS_PER_UNIT and/or DEFAULT_MAX_FEEDRATE. Disabling
* of max feedrate (M203) or resonant frequency (M593) may result feedrate * ADAPTIVE_STEP_SMOOTHING and reducing the step rate for non-shaped axes may
* being capped to prevent buffer overruns. * also reduce the buffer sizes. Runtime editing of max feedrate (M203) or
* resonant frequency (M593) may result in input shaping losing effectiveness
* during high speed movements to prevent buffer overruns.
* *
* Tune with M593 D<factor> F<frequency>: * Tune with M593 D<factor> F<frequency>:
* *
@ -1077,12 +1079,17 @@
* X<1> Set the given parameters only for the X axis. * X<1> Set the given parameters only for the X axis.
* Y<1> Set the given parameters only for the Y axis. * Y<1> Set the given parameters only for the Y axis.
*/ */
//#define INPUT_SHAPING //#define INPUT_SHAPING_X
#if ENABLED(INPUT_SHAPING) //#define INPUT_SHAPING_Y
#define SHAPING_FREQ_X 40 // (Hz) The dominant resonant frequency of the X axis. #if EITHER(INPUT_SHAPING_X, INPUT_SHAPING_Y)
#define SHAPING_FREQ_Y 40 // (Hz) The dominant resonant frequency of the Y axis. #if ENABLED(INPUT_SHAPING_X)
#define SHAPING_ZETA_X 0.3f // Damping ratio of the X axis (range: 0.0 = no damping to 1.0 = critical damping). #define SHAPING_FREQ_X 40 // (Hz) The default dominant resonant frequency on the X axis.
#define SHAPING_ZETA_Y 0.3f // Damping ratio of the Y axis (range: 0.0 = no damping to 1.0 = critical damping). #define SHAPING_ZETA_X 0.15f // Damping ratio of the X axis (range: 0.0 = no damping to 1.0 = critical damping).
#endif
#if ENABLED(INPUT_SHAPING_Y)
#define SHAPING_FREQ_Y 40 // (Hz) The default dominant resonant frequency on the Y axis.
#define SHAPING_ZETA_Y 0.15f // Damping ratio of the Y axis (range: 0.0 = no damping to 1.0 = critical damping).
#endif
//#define SHAPING_MENU // Add a menu to the LCD to set shaping parameters. //#define SHAPING_MENU // Add a menu to the LCD to set shaping parameters.
#endif #endif

21
Marlin/src/gcode/feature/input_shaping/M593.cpp
View File

@ -22,21 +22,21 @@
#include "../../../inc/MarlinConfig.h" #include "../../../inc/MarlinConfig.h"
#if ENABLED(INPUT_SHAPING) #if HAS_SHAPING
#include "../../gcode.h" #include "../../gcode.h"
#include "../../../module/stepper.h" #include "../../../module/stepper.h"
void GcodeSuite::M593_report(const bool forReplay/*=true*/) { void GcodeSuite::M593_report(const bool forReplay/*=true*/) {
report_heading_etc(forReplay, F("Input Shaping")); report_heading_etc(forReplay, F("Input Shaping"));
#if HAS_SHAPING_X #if ENABLED(INPUT_SHAPING_X)
SERIAL_ECHOLNPGM(" M593 X" SERIAL_ECHOLNPGM(" M593 X"
" F", stepper.get_shaping_frequency(X_AXIS), " F", stepper.get_shaping_frequency(X_AXIS),
" D", stepper.get_shaping_damping_ratio(X_AXIS) " D", stepper.get_shaping_damping_ratio(X_AXIS)
); );
#endif #endif
#if HAS_SHAPING_Y #if ENABLED(INPUT_SHAPING_Y)
TERN_(HAS_SHAPING_X, report_echo_start(forReplay)); TERN_(INPUT_SHAPING_X, report_echo_start(forReplay));
SERIAL_ECHOLNPGM(" M593 Y" SERIAL_ECHOLNPGM(" M593 Y"
" F", stepper.get_shaping_frequency(Y_AXIS), " F", stepper.get_shaping_frequency(Y_AXIS),
" D", stepper.get_shaping_damping_ratio(Y_AXIS) " D", stepper.get_shaping_damping_ratio(Y_AXIS)
@ -55,10 +55,10 @@ void GcodeSuite::M593_report(const bool forReplay/*=true*/) {
void GcodeSuite::M593() { void GcodeSuite::M593() {
if (!parser.seen_any()) return M593_report(); if (!parser.seen_any()) return M593_report();
const bool seen_X = TERN0(HAS_SHAPING_X, parser.seen_test('X')), const bool seen_X = TERN0(INPUT_SHAPING_X, parser.seen_test('X')),
seen_Y = TERN0(HAS_SHAPING_Y, parser.seen_test('Y')), seen_Y = TERN0(INPUT_SHAPING_Y, parser.seen_test('Y')),
for_X = seen_X || TERN0(HAS_SHAPING_X, (!seen_X && !seen_Y)), for_X = seen_X || TERN0(INPUT_SHAPING_X, (!seen_X && !seen_Y)),
for_Y = seen_Y || TERN0(HAS_SHAPING_Y, (!seen_X && !seen_Y)); for_Y = seen_Y || TERN0(INPUT_SHAPING_Y, (!seen_X && !seen_Y));
if (parser.seen('D')) { if (parser.seen('D')) {
const float zeta = parser.value_float(); const float zeta = parser.value_float();
@ -72,12 +72,13 @@ void GcodeSuite::M593() {
if (parser.seen('F')) { if (parser.seen('F')) {
const float freq = parser.value_float(); const float freq = parser.value_float();
if (freq > 0) { constexpr float max_freq = float(uint32_t(STEPPER_TIMER_RATE) / 2) / shaping_time_t(-2);
if (freq == 0.0f || freq > max_freq) {
if (for_X) stepper.set_shaping_frequency(X_AXIS, freq); if (for_X) stepper.set_shaping_frequency(X_AXIS, freq);
if (for_Y) stepper.set_shaping_frequency(Y_AXIS, freq); if (for_Y) stepper.set_shaping_frequency(Y_AXIS, freq);
} }
else else
SERIAL_ECHO_MSG("?Frequency (F) must be greater than 0"); SERIAL_ECHOLNPGM("?Frequency (F) must be greater than ", max_freq, " or 0 to disable");
} }
} }

2
Marlin/src/gcode/gcode.cpp
View File

@ -933,7 +933,7 @@ void GcodeSuite::process_parsed_command(const bool no_ok/*=false*/) {
case 575: M575(); break; // M575: Set serial baudrate case 575: M575(); break; // M575: Set serial baudrate
#endif #endif
#if ENABLED(INPUT_SHAPING) #if HAS_SHAPING
case 593: M593(); break; // M593: Set Input Shaping parameters case 593: M593(); break; // M593: Set Input Shaping parameters
#endif #endif

4
Marlin/src/gcode/gcode.h
View File

@ -259,7 +259,7 @@
* M554 - Get or set IP gateway. (Requires enabled Ethernet port) * 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) * 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) * M575 - Change the serial baud rate. (Requires BAUD_RATE_GCODE)
* M593 - Get or set input shaping parameters. (Requires INPUT_SHAPING) * M593 - Get or set input shaping parameters. (Requires INPUT_SHAPING_[XY])
* M600 - Pause for filament change: "M600 X<pos> Y<pos> Z<raise> E<first_retract> L<later_retract>". (Requires ADVANCED_PAUSE_FEATURE) * M600 - Pause for filament change: "M600 X<pos> Y<pos> Z<raise> E<first_retract> L<later_retract>". (Requires ADVANCED_PAUSE_FEATURE)
* M603 - Configure filament change: "M603 T<tool> U<unload_length> L<load_length>". (Requires ADVANCED_PAUSE_FEATURE) * M603 - Configure filament change: "M603 T<tool> U<unload_length> L<load_length>". (Requires ADVANCED_PAUSE_FEATURE)
* M605 - Set Dual X-Carriage movement mode: "M605 S<mode> [X<x_offset>] [R<temp_offset>]". (Requires DUAL_X_CARRIAGE) * M605 - Set Dual X-Carriage movement mode: "M605 S<mode> [X<x_offset>] [R<temp_offset>]". (Requires DUAL_X_CARRIAGE)
@ -1081,7 +1081,7 @@ private:
static void M575(); static void M575();
#endif #endif
#if ENABLED(INPUT_SHAPING) #if HAS_SHAPING
static void M593(); static void M593();
static void M593_report(const bool forReplay=true); static void M593_report(const bool forReplay=true);
#endif #endif

10
Marlin/src/inc/Conditionals_adv.h
View File

@ -1120,15 +1120,11 @@
#endif #endif
// Input shaping // Input shaping
#if ENABLED(INPUT_SHAPING)
#if !HAS_Y_AXIS #if !HAS_Y_AXIS
#undef INPUT_SHAPING_Y
#undef SHAPING_FREQ_Y #undef SHAPING_FREQ_Y
#undef SHAPING_BUFFER_Y #undef SHAPING_BUFFER_Y
#endif #endif
#ifdef SHAPING_FREQ_X #if EITHER(INPUT_SHAPING_X, INPUT_SHAPING_Y)
#define HAS_SHAPING_X 1 #define HAS_SHAPING 1
#endif
#ifdef SHAPING_FREQ_Y
#define HAS_SHAPING_Y 1
#endif
#endif #endif

14
Marlin/src/inc/SanityCheck.h
View File

@ -4271,14 +4271,14 @@ static_assert(_PLUS_TEST(4), "HOMING_FEEDRATE_MM_M values must be positive.");
#endif #endif
// Check requirements for Input Shaping // Check requirements for Input Shaping
#if ENABLED(INPUT_SHAPING) && defined(__AVR__) #if HAS_SHAPING && defined(__AVR__)
#if HAS_SHAPING_X #if ENABLED(INPUT_SHAPING_X)
#if F_CPU > 16000000 #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."); static_assert((SHAPING_FREQ_X) * 2 * 0x10000 >= (STEPPER_TIMER_RATE), "SHAPING_FREQ_X is below the minimum (20) for AVR 20MHz.");
#else #else
static_assert((SHAPING_FREQ_X) * 2 * 0x10000 >= (STEPPER_TIMER_RATE), "SHAPING_FREQ_X is below the minimum (16) for AVR 16MHz."); static_assert((SHAPING_FREQ_X) * 2 * 0x10000 >= (STEPPER_TIMER_RATE), "SHAPING_FREQ_X is below the minimum (16) for AVR 16MHz.");
#endif #endif
#elif HAS_SHAPING_Y #elif ENABLED(INPUT_SHAPING_Y)
#if F_CPU > 16000000 #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."); static_assert((SHAPING_FREQ_Y) * 2 * 0x10000 >= (STEPPER_TIMER_RATE), "SHAPING_FREQ_Y is below the minimum (20) for AVR 20MHz.");
#else #else
@ -4287,12 +4287,8 @@ static_assert(_PLUS_TEST(4), "HOMING_FEEDRATE_MM_M values must be positive.");
#endif #endif
#endif #endif
#if ENABLED(INPUT_SHAPING) #if BOTH(HAS_SHAPING, DIRECT_STEPPING)
#if ENABLED(DIRECT_STEPPING) #error "INPUT_SHAPING_[XY] cannot currently be used with 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 #endif
// Misc. Cleanup // Misc. Cleanup

8
Marlin/src/lcd/language/language_en.h
View File

@ -403,10 +403,10 @@ namespace Language_en {
LSTR MSG_A_RETRACT = _UxGT("Retract Accel"); LSTR MSG_A_RETRACT = _UxGT("Retract Accel");
LSTR MSG_A_TRAVEL = _UxGT("Travel Accel"); LSTR MSG_A_TRAVEL = _UxGT("Travel Accel");
LSTR MSG_INPUT_SHAPING = _UxGT("Input Shaping"); LSTR MSG_INPUT_SHAPING = _UxGT("Input Shaping");
LSTR MSG_SHAPING_X_FREQ = STR_X _UxGT(" frequency"); LSTR MSG_SHAPING_ENABLE = _UxGT("Enable @ shaping");
LSTR MSG_SHAPING_Y_FREQ = STR_Y _UxGT(" frequency"); LSTR MSG_SHAPING_DISABLE = _UxGT("Disable @ shaping");
LSTR MSG_SHAPING_X_ZETA = STR_X _UxGT(" damping"); LSTR MSG_SHAPING_FREQ = _UxGT("@ frequency");
LSTR MSG_SHAPING_Y_ZETA = STR_Y _UxGT(" damping"); LSTR MSG_SHAPING_ZETA = _UxGT("@ damping");
LSTR MSG_XY_FREQUENCY_LIMIT = _UxGT("XY Freq Limit"); LSTR MSG_XY_FREQUENCY_LIMIT = _UxGT("XY Freq Limit");
LSTR MSG_XY_FREQUENCY_FEEDRATE = _UxGT("Min FR Factor"); LSTR MSG_XY_FREQUENCY_FEEDRATE = _UxGT("Min FR Factor");
LSTR MSG_STEPS_PER_MM = _UxGT("Steps/mm"); LSTR MSG_STEPS_PER_MM = _UxGT("Steps/mm");

32
Marlin/src/lcd/menu/menu_advanced.cpp
View File

@ -545,24 +545,28 @@ void menu_backlash();
START_MENU(); START_MENU();
BACK_ITEM(MSG_ADVANCED_SETTINGS); BACK_ITEM(MSG_ADVANCED_SETTINGS);
// M593 F Frequency // M593 F Frequency and D Damping ratio
#if HAS_SHAPING_X #if ENABLED(INPUT_SHAPING_X)
editable.decimal = stepper.get_shaping_frequency(X_AXIS); 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); }); if (editable.decimal) {
#endif ACTION_ITEM_N(X_AXIS, MSG_SHAPING_DISABLE, []{ stepper.set_shaping_frequency(X_AXIS, 0.0f); });
#if HAS_SHAPING_Y EDIT_ITEM_FAST_N(float61, X_AXIS, MSG_SHAPING_FREQ, &editable.decimal, min_frequency, 200.0f, []{ stepper.set_shaping_frequency(X_AXIS, editable.decimal); });
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); 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); }); 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); });
#endif #endif
#if HAS_SHAPING_Y #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); });
EDIT_ITEM_FAST_N(float61, 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); 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); }); 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); });
#endif #endif
END_MENU(); END_MENU();

15
Marlin/src/module/planner.cpp
View File

@ -1724,6 +1724,13 @@ float Planner::triggered_position_mm(const AxisEnum axis) {
return result * mm_per_step[axis]; return result * mm_per_step[axis];
} }
bool Planner::busy() {
return (has_blocks_queued() || cleaning_buffer_counter
|| TERN0(EXTERNAL_CLOSED_LOOP_CONTROLLER, CLOSED_LOOP_WAITING())
|| TERN0(HAS_SHAPING, stepper.input_shaping_busy())
);
}
void Planner::finish_and_disable() { void Planner::finish_and_disable() {
while (has_blocks_queued() || cleaning_buffer_counter) idle(); while (has_blocks_queued() || cleaning_buffer_counter) idle();
stepper.disable_all_steppers(); stepper.disable_all_steppers();
@ -2483,14 +2490,6 @@ bool Planner::_populate_block(
#endif // XY_FREQUENCY_LIMIT #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 // Correct the speed
if (speed_factor < 1.0f) { if (speed_factor < 1.0f) {
current_speed *= speed_factor; current_speed *= speed_factor;

6
Marlin/src/module/planner.h
View File

@ -930,11 +930,7 @@ class Planner {
static float triggered_position_mm(const AxisEnum axis); static float triggered_position_mm(const AxisEnum axis);
// Blocks are queued, or we're running out moves, or the closed loop controller is waiting // Blocks are queued, or we're running out moves, or the closed loop controller is waiting
static bool busy() { static bool busy();
return (has_blocks_queued() || cleaning_buffer_counter
|| TERN0(EXTERNAL_CLOSED_LOOP_CONTROLLER, CLOSED_LOOP_WAITING())
);
}
// Block until all buffered steps are executed / cleaned // Block until all buffered steps are executed / cleaned
static void synchronize(); static void synchronize();

22
Marlin/src/module/settings.cpp
View File

@ -580,11 +580,11 @@ typedef struct SettingsDataStruct {
// //
// Input Shaping // Input Shaping
// //
#if HAS_SHAPING_X #if ENABLED(INPUT_SHAPING_X)
float shaping_x_frequency, // M593 X F float shaping_x_frequency, // M593 X F
shaping_x_zeta; // M593 X D shaping_x_zeta; // M593 X D
#endif #endif
#if HAS_SHAPING_Y #if ENABLED(INPUT_SHAPING_Y)
float shaping_y_frequency, // M593 Y F float shaping_y_frequency, // M593 Y F
shaping_y_zeta; // M593 Y D shaping_y_zeta; // M593 Y D
#endif #endif
@ -1617,12 +1617,12 @@ void MarlinSettings::postprocess() {
// //
// Input Shaping // Input Shaping
/// ///
#if ENABLED(INPUT_SHAPING) #if HAS_SHAPING
#if HAS_SHAPING_X #if ENABLED(INPUT_SHAPING_X)
EEPROM_WRITE(stepper.get_shaping_frequency(X_AXIS)); EEPROM_WRITE(stepper.get_shaping_frequency(X_AXIS));
EEPROM_WRITE(stepper.get_shaping_damping_ratio(X_AXIS)); EEPROM_WRITE(stepper.get_shaping_damping_ratio(X_AXIS));
#endif #endif
#if HAS_SHAPING_Y #if ENABLED(INPUT_SHAPING_Y)
EEPROM_WRITE(stepper.get_shaping_frequency(Y_AXIS)); EEPROM_WRITE(stepper.get_shaping_frequency(Y_AXIS));
EEPROM_WRITE(stepper.get_shaping_damping_ratio(Y_AXIS)); EEPROM_WRITE(stepper.get_shaping_damping_ratio(Y_AXIS));
#endif #endif
@ -2602,7 +2602,7 @@ void MarlinSettings::postprocess() {
// //
// Input Shaping // Input Shaping
// //
#if HAS_SHAPING_X #if ENABLED(INPUT_SHAPING_X)
{ {
float _data[2]; float _data[2];
EEPROM_READ(_data); EEPROM_READ(_data);
@ -2611,7 +2611,7 @@ void MarlinSettings::postprocess() {
} }
#endif #endif
#if HAS_SHAPING_Y #if ENABLED(INPUT_SHAPING_Y)
{ {
float _data[2]; float _data[2];
EEPROM_READ(_data); EEPROM_READ(_data);
@ -3389,12 +3389,12 @@ void MarlinSettings::reset() {
// //
// Input Shaping // Input Shaping
// //
#if ENABLED(INPUT_SHAPING) #if HAS_SHAPING
#if HAS_SHAPING_X #if ENABLED(INPUT_SHAPING_X)
stepper.set_shaping_frequency(X_AXIS, SHAPING_FREQ_X); stepper.set_shaping_frequency(X_AXIS, SHAPING_FREQ_X);
stepper.set_shaping_damping_ratio(X_AXIS, SHAPING_ZETA_X); stepper.set_shaping_damping_ratio(X_AXIS, SHAPING_ZETA_X);
#endif #endif
#if HAS_SHAPING_Y #if ENABLED(INPUT_SHAPING_Y)
stepper.set_shaping_frequency(Y_AXIS, SHAPING_FREQ_Y); stepper.set_shaping_frequency(Y_AXIS, SHAPING_FREQ_Y);
stepper.set_shaping_damping_ratio(Y_AXIS, SHAPING_ZETA_Y); stepper.set_shaping_damping_ratio(Y_AXIS, SHAPING_ZETA_Y);
#endif #endif
@ -3650,7 +3650,7 @@ void MarlinSettings::reset() {
// //
// Input Shaping // Input Shaping
// //
TERN_(INPUT_SHAPING, gcode.M593_report(forReplay)); TERN_(HAS_SHAPING, gcode.M593_report(forReplay));
// //
// Linear Advance // Linear Advance

354
Marlin/src/module/stepper.cpp
View File

@ -232,16 +232,24 @@ uint32_t Stepper::advance_divisor = 0,
Stepper::la_advance_steps = 0; Stepper::la_advance_steps = 0;
#endif #endif
#if ENABLED(INPUT_SHAPING) #if HAS_SHAPING
shaping_time_t DelayTimeManager::now = 0; shaping_time_t ShapingQueue::now = 0;
ParamDelayQueue Stepper::shaping_dividend_queue; shaping_time_t ShapingQueue::times[shaping_echoes];
DelayQueue<shaping_dividends> Stepper::shaping_queue; shaping_echo_axis_t ShapingQueue::echo_axes[shaping_echoes];
#if HAS_SHAPING_X uint16_t ShapingQueue::tail = 0;
shaping_time_t DelayTimeManager::delay_x;
#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; ShapeParams Stepper::shaping_x;
#endif #endif
#if HAS_SHAPING_Y #if ENABLED(INPUT_SHAPING_Y)
shaping_time_t DelayTimeManager::delay_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; ShapeParams Stepper::shaping_y;
#endif #endif
#endif #endif
@ -1479,20 +1487,10 @@ void Stepper::isr() {
// Enable ISRs to reduce USART processing latency // Enable ISRs to reduce USART processing latency
hal.isr_on(); hal.isr_on();
#if ENABLED(INPUT_SHAPING) TERN_(HAS_SHAPING, shaping_isr()); // Do Shaper stepping, if needed
// 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 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 ENABLED(LIN_ADVANCE)
if (!nextAdvanceISR) { // 0 = Do Linear Advance E Stepper pulses if (!nextAdvanceISR) { // 0 = Do Linear Advance E Stepper pulses
advance_isr(); advance_isr();
@ -1523,10 +1521,8 @@ void Stepper::isr() {
const uint32_t interval = _MIN( const uint32_t interval = _MIN(
uint32_t(HAL_TIMER_TYPE_MAX), // Come back in a very long time uint32_t(HAL_TIMER_TYPE_MAX), // Come back in a very long time
nextMainISR // Time until the next Pulse / Block phase 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(INPUT_SHAPING_X, ShapingQueue::peek_x()) // Time until next input shaping echo for X
OPTARG(HAS_SHAPING_Y, shaping_dividend_queue.peek_y()) // Time until next input shaping dividend change for Y OPTARG(INPUT_SHAPING_Y, ShapingQueue::peek_y()) // Time until next input shaping echo 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(LIN_ADVANCE, nextAdvanceISR) // Come back early for Linear Advance?
OPTARG(INTEGRATED_BABYSTEPPING, nextBabystepISR) // Come back early for Babystepping? OPTARG(INTEGRATED_BABYSTEPPING, nextBabystepISR) // Come back early for Babystepping?
); );
@ -1539,16 +1535,9 @@ void Stepper::isr() {
// //
nextMainISR -= interval; nextMainISR -= interval;
TERN_(HAS_SHAPING, ShapingQueue::decrement_delays(interval));
TERN_(INPUT_SHAPING, DelayTimeManager::decrement_delays(interval)); TERN_(LIN_ADVANCE, if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval);
TERN_(INTEGRATED_BABYSTEPPING, if (nextBabystepISR != BABYSTEP_NEVER) nextBabystepISR -= interval);
#if ENABLED(LIN_ADVANCE)
if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval;
#endif
#if ENABLED(INTEGRATED_BABYSTEPPING)
if (nextBabystepISR != BABYSTEP_NEVER) nextBabystepISR -= interval;
#endif
/** /**
* This needs to avoid a race-condition caused by interleaving * This needs to avoid a race-condition caused by interleaving
@ -1636,11 +1625,16 @@ void Stepper::pulse_phase_isr() {
abort_current_block = false; abort_current_block = false;
if (current_block) { if (current_block) {
discard_current_block(); discard_current_block();
#if ENABLED(INPUT_SHAPING) #if HAS_SHAPING
shaping_dividend_queue.purge(); ShapingQueue::purge();
shaping_queue.purge(); #if ENABLED(INPUT_SHAPING_X)
TERN_(HAS_SHAPING_X, delta_error.x = 0); shaping_x.delta_error = 0;
TERN_(HAS_SHAPING_Y, delta_error.y = 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
#endif #endif
} }
} }
@ -1676,31 +1670,48 @@ void Stepper::pulse_phase_isr() {
#define PULSE_PREP(AXIS) do{ \ #define PULSE_PREP(AXIS) do{ \
delta_error[_AXIS(AXIS)] += advance_dividend[_AXIS(AXIS)]; \ delta_error[_AXIS(AXIS)] += advance_dividend[_AXIS(AXIS)]; \
step_needed[_AXIS(AXIS)] = (delta_error[_AXIS(AXIS)] >= 0); \ step_needed[_AXIS(AXIS)] = (delta_error[_AXIS(AXIS)] >= 0); \
if (step_needed[_AXIS(AXIS)]) { \ if (step_needed[_AXIS(AXIS)]) \
count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \
delta_error[_AXIS(AXIS)] -= advance_divisor; \ delta_error[_AXIS(AXIS)] -= advance_divisor; \
} \
}while(0) }while(0)
#define PULSE_PREP_SHAPING(AXIS, DIVIDEND) do{ \ // With input shaping, direction changes can happen with almost only
delta_error[_AXIS(AXIS)] += (DIVIDEND); \ // AWAIT_LOW_PULSE() and DIR_WAIT_BEFORE() between steps. To work around
if ((MAXDIR(AXIS) && delta_error[_AXIS(AXIS)] <= -0x30000000L) || (MINDIR(AXIS) && delta_error[_AXIS(AXIS)] >= 0x30000000L)) { \ // 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)
#define HYSTERESIS_X 64
#else
#define HYSTERESIS_X 0
#endif
#if AXIS_DRIVER_TYPE_Y(TMC2208) || AXIS_DRIVER_TYPE_Y(TMC2208_STANDALONE)
#define HYSTERESIS_Y 64
#else
#define HYSTERESIS_Y 0
#endif
#define _HYSTERESIS(AXIS) HYSTERESIS_##AXIS
#define HYSTERESIS(AXIS) _HYSTERESIS(AXIS)
#define PULSE_PREP_SHAPING(AXIS, DELTA_ERROR, DIVIDEND) do{ \
if (step_needed[_AXIS(AXIS)]) { \
DELTA_ERROR += (DIVIDEND); \
if ((MAXDIR(AXIS) && DELTA_ERROR <= -(64 + HYSTERESIS(AXIS))) || (MINDIR(AXIS) && DELTA_ERROR >= (64 + HYSTERESIS(AXIS)))) { \
{ USING_TIMED_PULSE(); START_TIMED_PULSE(); AWAIT_LOW_PULSE(); } \
TBI(last_direction_bits, _AXIS(AXIS)); \ TBI(last_direction_bits, _AXIS(AXIS)); \
DIR_WAIT_BEFORE(); \ DIR_WAIT_BEFORE(); \
SET_STEP_DIR(AXIS); \ SET_STEP_DIR(AXIS); \
DIR_WAIT_AFTER(); \ DIR_WAIT_AFTER(); \
} \ } \
step_needed[_AXIS(AXIS)] = (MAXDIR(AXIS) && delta_error[_AXIS(AXIS)] >= 0x10000000L) || \ step_needed[_AXIS(AXIS)] = DELTA_ERROR <= -(64 + HYSTERESIS(AXIS)) || DELTA_ERROR >= (64 + HYSTERESIS(AXIS)); \
(MINDIR(AXIS) && delta_error[_AXIS(AXIS)] <= -0x10000000L); \ if (step_needed[_AXIS(AXIS)]) \
if (step_needed[_AXIS(AXIS)]) { \ DELTA_ERROR += MAXDIR(AXIS) ? -128 : 128; \
count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \
delta_error[_AXIS(AXIS)] += MAXDIR(AXIS) ? -0x20000000L : 0x20000000L; \
} \ } \
}while(0) }while(0)
// Start an active pulse if needed // Start an active pulse if needed
#define PULSE_START(AXIS) do{ \ #define PULSE_START(AXIS) do{ \
if (step_needed[_AXIS(AXIS)]) { \ if (step_needed[_AXIS(AXIS)]) { \
count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \
_APPLY_STEP(AXIS, !_INVERT_STEP_PIN(AXIS), 0); \ _APPLY_STEP(AXIS, !_INVERT_STEP_PIN(AXIS), 0); \
} \ } \
}while(0) }while(0)
@ -1819,23 +1830,13 @@ void Stepper::pulse_phase_isr() {
#endif // DIRECT_STEPPING #endif // DIRECT_STEPPING
if (!is_page) { if (!is_page) {
TERN_(INPUT_SHAPING, shaping_queue.enqueue());
// Determine if pulses are needed // Determine if pulses are needed
#if HAS_X_STEP #if HAS_X_STEP
#if HAS_SHAPING_X
PULSE_PREP_SHAPING(X, advance_dividend.x);
#else
PULSE_PREP(X); PULSE_PREP(X);
#endif #endif
#endif
#if HAS_Y_STEP #if HAS_Y_STEP
#if HAS_SHAPING_Y
PULSE_PREP_SHAPING(Y, advance_dividend.y);
#else
PULSE_PREP(Y); PULSE_PREP(Y);
#endif #endif
#endif
#if HAS_Z_STEP #if HAS_Z_STEP
PULSE_PREP(Z); PULSE_PREP(Z);
#endif #endif
@ -1871,6 +1872,24 @@ void Stepper::pulse_phase_isr() {
} }
#endif #endif
#endif #endif
#if HAS_SHAPING
// record an echo if a step is needed in the primary bresenham
const bool x_step = TERN0(INPUT_SHAPING_X, shaping_x.enabled && step_needed[X_AXIS]),
y_step = TERN0(INPUT_SHAPING_Y, shaping_y.enabled && step_needed[Y_AXIS]);
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));
// do the first part of the secondary bresenham
#if ENABLED(INPUT_SHAPING_X)
if (shaping_x.enabled)
PULSE_PREP_SHAPING(X, shaping_x.delta_error, shaping_x.factor1 * (shaping_x.forward ? 1 : -1));
#endif
#if ENABLED(INPUT_SHAPING_Y)
if (shaping_y.enabled)
PULSE_PREP_SHAPING(Y, shaping_y.delta_error, shaping_y.factor1 * (shaping_y.forward ? 1 : -1));
#endif
#endif
} }
#if ISR_MULTI_STEPS #if ISR_MULTI_STEPS
@ -1910,7 +1929,10 @@ void Stepper::pulse_phase_isr() {
#endif #endif
#if ENABLED(MIXING_EXTRUDER) #if ENABLED(MIXING_EXTRUDER)
if (step_needed.e) E_STEP_WRITE(mixer.get_next_stepper(), !INVERT_E_STEP_PIN); if (step_needed.e) {
count_position[E_AXIS] += count_direction[E_AXIS];
E_STEP_WRITE(mixer.get_next_stepper(), !INVERT_E_STEP_PIN);
}
#elif HAS_E0_STEP #elif HAS_E0_STEP
PULSE_START(E); PULSE_START(E);
#endif #endif
@ -1965,55 +1987,59 @@ void Stepper::pulse_phase_isr() {
} while (--events_to_do); } while (--events_to_do);
} }
#if ENABLED(INPUT_SHAPING) #if HAS_SHAPING
void Stepper::shaping_isr() { void Stepper::shaping_isr() {
xyze_bool_t step_needed{0}; xy_bool_t step_needed{0};
const bool shapex = TERN0(HAS_SHAPING_X, !shaping_queue.peek_x()), // Clear the echoes that are ready to process. If the buffers are too full and risk overflo, also apply echoes early.
shapey = TERN0(HAS_SHAPING_Y, !shaping_queue.peek_y()); TERN_(INPUT_SHAPING_X, step_needed[X_AXIS] = !ShapingQueue::peek_x() || ShapingQueue::free_count_x() < steps_per_isr);
TERN_(INPUT_SHAPING_Y, step_needed[Y_AXIS] = !ShapingQueue::peek_y() || ShapingQueue::free_count_y() < steps_per_isr);
#if HAS_SHAPING_X if (bool(step_needed)) while (true) {
if (!shaping_dividend_queue.peek_x()) shaping_x.dividend = shaping_dividend_queue.dequeue_x(); #if ENABLED(INPUT_SHAPING_X)
#endif if (step_needed[X_AXIS]) {
#if HAS_SHAPING_Y const bool forward = ShapingQueue::dequeue_x();
if (!shaping_dividend_queue.peek_y()) shaping_y.dividend = shaping_dividend_queue.dequeue_y(); PULSE_PREP_SHAPING(X, shaping_x.delta_error, shaping_x.factor2 * (forward ? 1 : -1));
#endif
#if HAS_SHAPING_X
if (shapex) {
shaping_queue.dequeue_x();
PULSE_PREP_SHAPING(X, shaping_x.dividend);
PULSE_START(X); PULSE_START(X);
} }
#endif #endif
#if HAS_SHAPING_Y #if ENABLED(INPUT_SHAPING_Y)
if (shapey) { if (step_needed[Y_AXIS]) {
shaping_queue.dequeue_y(); const bool forward = ShapingQueue::dequeue_y();
PULSE_PREP_SHAPING(Y, shaping_y.dividend); PULSE_PREP_SHAPING(Y, shaping_y.delta_error, shaping_y.factor2 * (forward ? 1 : -1));
PULSE_START(Y); PULSE_START(Y);
} }
#endif #endif
TERN_(I2S_STEPPER_STREAM, i2s_push_sample()); TERN_(I2S_STEPPER_STREAM, i2s_push_sample());
if (shapex || shapey) {
#if ISR_MULTI_STEPS
USING_TIMED_PULSE(); USING_TIMED_PULSE();
if (bool(step_needed)) {
#if ISR_MULTI_STEPS
START_TIMED_PULSE(); START_TIMED_PULSE();
AWAIT_HIGH_PULSE(); AWAIT_HIGH_PULSE();
#endif #endif
#if HAS_SHAPING_X #if ENABLED(INPUT_SHAPING_X)
if (shapex) PULSE_STOP(X); PULSE_STOP(X);
#endif #endif
#if HAS_SHAPING_Y #if ENABLED(INPUT_SHAPING_Y)
if (shapey) PULSE_STOP(Y); PULSE_STOP(Y);
#endif #endif
} }
TERN_(INPUT_SHAPING_X, step_needed[X_AXIS] = !ShapingQueue::peek_x() || ShapingQueue::free_count_x() < steps_per_isr);
TERN_(INPUT_SHAPING_Y, step_needed[Y_AXIS] = !ShapingQueue::peek_y() || ShapingQueue::free_count_y() < steps_per_isr);
if (!bool(step_needed)) break;
START_TIMED_PULSE();
AWAIT_LOW_PULSE();
}
} }
#endif // INPUT_SHAPING #endif // HAS_SHAPING
// Calculate timer interval, with all limits applied. // Calculate timer interval, with all limits applied.
uint32_t Stepper::calc_timer_interval(uint32_t step_rate) { uint32_t Stepper::calc_timer_interval(uint32_t step_rate) {
@ -2462,79 +2488,55 @@ uint32_t Stepper::block_phase_isr() {
acceleration_time = deceleration_time = 0; acceleration_time = deceleration_time = 0;
#if ENABLED(ADAPTIVE_STEP_SMOOTHING) #if ENABLED(ADAPTIVE_STEP_SMOOTHING)
uint8_t oversampling = 0; // Assume no axis smoothing (via oversampling) oversampling_factor = 0; // Assume no axis smoothing (via oversampling)
// Decide if axis smoothing is possible // Decide if axis smoothing is possible
uint32_t max_rate = current_block->nominal_rate; // Get the step event rate 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... while (max_rate < MIN_STEP_ISR_FREQUENCY) { // As long as more ISRs are possible...
max_rate <<= 1; // Try to double the rate max_rate <<= 1; // Try to double the rate
if (max_rate < MIN_STEP_ISR_FREQUENCY) // Don't exceed the estimated ISR limit if (max_rate < MIN_STEP_ISR_FREQUENCY) // Don't exceed the estimated ISR limit
++oversampling; // Increase the oversampling (used for left-shift) ++oversampling_factor; // Increase the oversampling (used for left-shift)
} }
oversampling_factor = oversampling; // For all timer interval calculations
#else
constexpr uint8_t oversampling = 0;
#endif #endif
// Based on the oversampling factor, do the calculations // Based on the oversampling factor, do the calculations
step_event_count = current_block->step_event_count << oversampling; step_event_count = current_block->step_event_count << oversampling_factor;
// Initialize Bresenham delta errors to 1/2 // 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); delta_error = TERN_(LIN_ADVANCE, la_delta_error =) -int32_t(step_event_count);
// Calculate Bresenham dividends and divisors // Calculate Bresenham dividends and divisors
advance_dividend = (current_block->steps << 1).asLong(); advance_dividend = (current_block->steps << 1).asLong();
advance_divisor = step_event_count << 1; advance_divisor = step_event_count << 1;
// for input shaped axes, advance_divisor is replaced with 0x40000000 #if ENABLED(INPUT_SHAPING_X)
// and steps are repeated twice so dividends have to be scaled and halved if (shaping_x.enabled) {
// and the dividend is directional, i.e. signed const int64_t steps = TEST(current_block->direction_bits, X_AXIS) ? -int64_t(current_block->steps.x) : int64_t(current_block->steps.x);
TERN_(HAS_SHAPING_X, advance_dividend.x = (uint64_t(current_block->steps.x) << 29) / step_event_count); shaping_x.last_block_end_pos += steps;
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. // If there are any remaining echos unprocessed, then direction change must
// For this segment, there will be step_event_count calls to the Bresenham logic and the same number of echoes. // be delayed and processed in PULSE_PREP_SHAPING. This will cause half a step
// For each pair of calls to the Bresenham logic, delta_error will increase by advance_dividend modulo 0x20000000 // to be missed, which will need recovering and this can be done through shaping_x.remainder.
// so (e.g. for x) delta_error.x will end up changing by (advance_dividend.x * step_event_count) % 0x20000000. shaping_x.forward = !TEST(current_block->direction_bits, X_AXIS);
// For a divisor which is a power of 2, modulo is the same as as a bitmask, i.e. if (!ShapingQueue::empty_x()) SET_BIT_TO(current_block->direction_bits, X_AXIS, TEST(last_direction_bits, X_AXIS));
// (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 #endif
// plan the change of values for advance_dividend for the input shaping echoes // Y follows the same logic as X (but the comments aren't repeated)
TERN_(INPUT_SHAPING, shaping_dividend_queue.enqueue(TERN0(HAS_SHAPING_X, echo_x), TERN0(HAS_SHAPING_Y, echo_y))); #if ENABLED(INPUT_SHAPING_Y)
if (shaping_y.enabled) {
// apply the adjustment to the primary signal const int64_t steps = TEST(current_block->direction_bits, Y_AXIS) ? -int64_t(current_block->steps.y) : int64_t(current_block->steps.y);
TERN_(HAS_SHAPING_X, advance_dividend.x -= echo_x); shaping_y.last_block_end_pos += steps;
TERN_(HAS_SHAPING_Y, advance_dividend.y -= echo_y); shaping_y.forward = !TEST(current_block->direction_bits, Y_AXIS);
if (!ShapingQueue::empty_y()) SET_BIT_TO(current_block->direction_bits, Y_AXIS, TEST(last_direction_bits, Y_AXIS));
}
#endif
// No step events completed so far // No step events completed so far
step_events_completed = 0; step_events_completed = 0;
// Compute the acceleration and deceleration points // Compute the acceleration and deceleration points
accelerate_until = current_block->accelerate_until << oversampling; accelerate_until = current_block->accelerate_until << oversampling_factor;
decelerate_after = current_block->decelerate_after << oversampling; decelerate_after = current_block->decelerate_after << oversampling_factor;
TERN_(MIXING_EXTRUDER, mixer.stepper_setup(current_block->b_color)); TERN_(MIXING_EXTRUDER, mixer.stepper_setup(current_block->b_color));
@ -2548,7 +2550,7 @@ uint32_t Stepper::block_phase_isr() {
#endif #endif
if (current_block->la_advance_rate) { if (current_block->la_advance_rate) {
// apply LA scaling and discount the effect of frequency scaling // apply LA scaling and discount the effect of frequency scaling
la_dividend = (advance_dividend.e << current_block->la_scaling) << oversampling; la_dividend = (advance_dividend.e << current_block->la_scaling) << oversampling_factor;
} }
#endif #endif
@ -2974,7 +2976,8 @@ void Stepper::init() {
#endif #endif
} }
#if ENABLED(INPUT_SHAPING) #if HAS_SHAPING
/** /**
* Calculate a fixed point factor to apply to the signal and its echo * Calculate a fixed point factor to apply to the signal and its echo
* when shaping an axis. * when shaping an axis.
@ -2983,41 +2986,68 @@ void Stepper::init() {
// from the damping ratio, get a factor that can be applied to advance_dividend for fixed point maths // 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)) // 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 // which can be converted to 1:7 fixed point with an excellent fit with a 3rd order polynomial
float shaping_factor; float factor2;
if (zeta <= 0.0f) shaping_factor = 64.0f; if (zeta <= 0.0f) factor2 = 64.0f;
else if (zeta >= 1.0f) shaping_factor = 0.0f; else if (zeta >= 1.0f) factor2 = 0.0f;
else { else {
shaping_factor = 64.44056192 + -99.02008832 * zeta; factor2 = 64.44056192 + -99.02008832 * zeta;
const float zeta2 = zeta * zeta; const float zeta2 = zeta * zeta;
shaping_factor += -7.58095488 * zeta2; factor2 += -7.58095488 * zeta2;
const float zeta3 = zeta2 * zeta; const float zeta3 = zeta2 * zeta;
shaping_factor += 43.073216 * zeta3; factor2 += 43.073216 * zeta3;
factor2 = floor(factor2);
} }
const bool was_on = hal.isr_state(); const bool was_on = hal.isr_state();
hal.isr_off(); hal.isr_off();
TERN_(HAS_SHAPING_X, if (axis == X_AXIS) { shaping_x.factor = floor(shaping_factor); shaping_x.zeta = zeta; }) TERN_(INPUT_SHAPING_X, if (axis == X_AXIS) { shaping_x.factor2 = factor2; shaping_x.factor1 = 128 - factor2; shaping_x.zeta = zeta; })
TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) { shaping_y.factor = floor(shaping_factor); shaping_y.zeta = zeta; }) TERN_(INPUT_SHAPING_Y, if (axis == Y_AXIS) { shaping_y.factor2 = factor2; shaping_y.factor1 = 128 - factor2; shaping_y.zeta = zeta; })
if (was_on) hal.isr_on(); if (was_on) hal.isr_on();
} }
float Stepper::get_shaping_damping_ratio(const AxisEnum axis) { float Stepper::get_shaping_damping_ratio(const AxisEnum axis) {
TERN_(HAS_SHAPING_X, if (axis == X_AXIS) return shaping_x.zeta); TERN_(INPUT_SHAPING_X, if (axis == X_AXIS) return shaping_x.zeta);
TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) return shaping_y.zeta); TERN_(INPUT_SHAPING_Y, if (axis == Y_AXIS) return shaping_y.zeta);
return -1; return -1;
} }
void Stepper::set_shaping_frequency(const AxisEnum axis, const float freq) { 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; }) // enabling or disabling shaping whilst moving can result in lost steps
TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) { DelayTimeManager::set_delay(axis, float(uint32_t(STEPPER_TIMER_RATE) / 2) / freq); shaping_y.frequency = freq; }) Planner::synchronize();
const bool was_on = hal.isr_state();
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;
}
#endif
if (was_on) hal.isr_on();
} }
float Stepper::get_shaping_frequency(const AxisEnum axis) { float Stepper::get_shaping_frequency(const AxisEnum axis) {
TERN_(HAS_SHAPING_X, if (axis == X_AXIS) return shaping_x.frequency); TERN_(INPUT_SHAPING_X, if (axis == X_AXIS) return shaping_x.frequency);
TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) return shaping_y.frequency); TERN_(INPUT_SHAPING_Y, if (axis == Y_AXIS) return shaping_y.frequency);
return -1; return -1;
} }
#endif
#endif // HAS_SHAPING
/** /**
* Set the stepper positions directly in steps * Set the stepper positions directly in steps
@ -3029,6 +3059,13 @@ void Stepper::init() {
* derive the current XYZE position later on. * derive the current XYZE position later on.
*/ */
void Stepper::_set_position(const abce_long_t &spos) { void Stepper::_set_position(const abce_long_t &spos) {
#if ENABLED(INPUT_SHAPING_X)
const int32_t x_shaping_delta = count_position.x - shaping_x.last_block_end_pos;
#endif
#if ENABLED(INPUT_SHAPING_Y)
const int32_t y_shaping_delta = count_position.y - shaping_y.last_block_end_pos;
#endif
#if ANY(IS_CORE, MARKFORGED_XY, MARKFORGED_YX) #if ANY(IS_CORE, MARKFORGED_XY, MARKFORGED_YX)
#if CORE_IS_XY #if CORE_IS_XY
// corexy positioning // corexy positioning
@ -3058,6 +3095,19 @@ void Stepper::_set_position(const abce_long_t &spos) {
// default non-h-bot planning // default non-h-bot planning
count_position = spos; count_position = spos;
#endif #endif
#if ENABLED(INPUT_SHAPING_X)
if (shaping_x.enabled) {
count_position.x += x_shaping_delta;
shaping_x.last_block_end_pos = spos.x;
}
#endif
#if ENABLED(INPUT_SHAPING_Y)
if (shaping_y.enabled) {
count_position.y += y_shaping_delta;
shaping_y.last_block_end_pos = spos.y;
}
#endif
} }
/** /**
@ -3097,6 +3147,8 @@ void Stepper::set_axis_position(const AxisEnum a, const int32_t &v) {
#endif #endif
count_position[a] = 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);
#ifdef __AVR__ #ifdef __AVR__
// Reenable Stepper ISR // Reenable Stepper ISR

236
Marlin/src/module/stepper.h
View File

@ -75,8 +75,8 @@
*/ */
#define TIMER_READ_ADD_AND_STORE_CYCLES 34UL #define TIMER_READ_ADD_AND_STORE_CYCLES 34UL
// The base ISR takes 792 cycles // The base ISR
#define ISR_BASE_CYCLES 792UL #define ISR_BASE_CYCLES 770UL
// Linear advance base time is 64 cycles // Linear advance base time is 64 cycles
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
@ -92,21 +92,25 @@
#define ISR_S_CURVE_CYCLES 0UL #define ISR_S_CURVE_CYCLES 0UL
#endif #endif
// Input shaping base time
#if HAS_SHAPING
#define ISR_SHAPING_BASE_CYCLES 180UL
#else
#define ISR_SHAPING_BASE_CYCLES 0UL
#endif
// Stepper Loop base cycles // Stepper Loop base cycles
#define ISR_LOOP_BASE_CYCLES 4UL #define ISR_LOOP_BASE_CYCLES 4UL
// To start the step pulse, in the worst case takes
#define ISR_START_STEPPER_CYCLES 13UL
// And each stepper (start + stop pulse) takes in worst case // And each stepper (start + stop pulse) takes in worst case
#define ISR_STEPPER_CYCLES 16UL #define ISR_STEPPER_CYCLES 100UL
#else #else
// Cycles to perform actions in START_TIMED_PULSE // Cycles to perform actions in START_TIMED_PULSE
#define TIMER_READ_ADD_AND_STORE_CYCLES 13UL #define TIMER_READ_ADD_AND_STORE_CYCLES 13UL
// The base ISR takes 752 cycles // The base ISR
#define ISR_BASE_CYCLES 752UL #define ISR_BASE_CYCLES 1000UL
// Linear advance base time is 32 cycles // Linear advance base time is 32 cycles
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
@ -122,12 +126,16 @@
#define ISR_S_CURVE_CYCLES 0UL #define ISR_S_CURVE_CYCLES 0UL
#endif #endif
// Input shaping base time
#if HAS_SHAPING
#define ISR_SHAPING_BASE_CYCLES 290UL
#else
#define ISR_SHAPING_BASE_CYCLES 0UL
#endif
// Stepper Loop base cycles // Stepper Loop base cycles
#define ISR_LOOP_BASE_CYCLES 32UL #define ISR_LOOP_BASE_CYCLES 32UL
// To start the step pulse, in the worst case takes
#define ISR_START_STEPPER_CYCLES 57UL
// And each stepper (start + stop pulse) takes in worst case // And each stepper (start + stop pulse) takes in worst case
#define ISR_STEPPER_CYCLES 88UL #define ISR_STEPPER_CYCLES 88UL
@ -202,8 +210,12 @@
#error "Expected at least one of MINIMUM_STEPPER_PULSE or MAXIMUM_STEPPER_RATE to be defined" #error "Expected at least one of MINIMUM_STEPPER_PULSE or MAXIMUM_STEPPER_RATE to be defined"
#endif #endif
// But the user could be enforcing a minimum time, so the loop time is // The loop takes the base time plus the time for all the bresenham logic for R pulses plus the time
#define ISR_LOOP_CYCLES (ISR_LOOP_BASE_CYCLES + _MAX(MIN_STEPPER_PULSE_CYCLES, MIN_ISR_LOOP_CYCLES)) // between pulses for (R-1) pulses. But the user could be enforcing a minimum time so the loop time is:
#define ISR_LOOP_CYCLES(R) ((ISR_LOOP_BASE_CYCLES + MIN_ISR_LOOP_CYCLES + MIN_STEPPER_PULSE_CYCLES) * (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_SHAPING, ISR_LOOP_BASE_CYCLES) + TERN0(INPUT_SHAPING_X, ISR_X_STEPPER_CYCLES) + TERN0(INPUT_SHAPING_Y, ISR_Y_STEPPER_CYCLES)) * (R) + (MIN_ISR_LOOP_CYCLES) * (R - 1))
// If linear advance is enabled, then it is handled separately // If linear advance is enabled, then it is handled separately
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
@ -228,7 +240,7 @@
#endif #endif
// Now estimate the total ISR execution time in cycles given a step per ISR multiplier // Now estimate the total ISR execution time in cycles given a step per ISR multiplier
#define ISR_EXECUTION_CYCLES(R) (((ISR_BASE_CYCLES + ISR_S_CURVE_CYCLES + (ISR_LOOP_CYCLES) * (R) + ISR_LA_BASE_CYCLES + ISR_LA_LOOP_CYCLES)) / (R)) #define ISR_EXECUTION_CYCLES(R) (((ISR_BASE_CYCLES + ISR_S_CURVE_CYCLES + ISR_SHAPING_BASE_CYCLES + ISR_LOOP_CYCLES(R) + ISR_SHAPING_LOOP_CYCLES(R) + ISR_LA_BASE_CYCLES + ISR_LA_LOOP_CYCLES)) / (R))
// The maximum allowable stepping frequency when doing x128-x1 stepping (in Hz) // The maximum allowable stepping frequency when doing x128-x1 stepping (in Hz)
#define MAX_STEP_ISR_FREQUENCY_128X ((F_CPU) / ISR_EXECUTION_CYCLES(128)) #define MAX_STEP_ISR_FREQUENCY_128X ((F_CPU) / ISR_EXECUTION_CYCLES(128))
@ -312,14 +324,15 @@ constexpr ena_mask_t enable_overlap[] = {
//static_assert(!any_enable_overlap(), "There is some overlap."); //static_assert(!any_enable_overlap(), "There is some overlap.");
#if ENABLED(INPUT_SHAPING) #if HAS_SHAPING
typedef IF<ENABLED(__AVR__), uint16_t, uint32_t>::type shaping_time_t;
// These constexpr are used to calculate the shaping queue buffer sizes // These constexpr are used to calculate the shaping queue buffer sizes
constexpr xyze_float_t max_feedrate = DEFAULT_MAX_FEEDRATE; constexpr xyze_float_t max_feedrate = DEFAULT_MAX_FEEDRATE;
constexpr xyze_float_t steps_per_unit = DEFAULT_AXIS_STEPS_PER_UNIT; constexpr xyze_float_t steps_per_unit = DEFAULT_AXIS_STEPS_PER_UNIT;
constexpr float max_steprate = _MAX(LOGICAL_AXIS_LIST( // MIN_STEP_ISR_FREQUENCY is known at compile time on AVRs and any reduction in SRAM is welcome
#ifdef __AVR__
constexpr float max_isr_rate = _MAX(
LOGICAL_AXIS_LIST(
max_feedrate.e * steps_per_unit.e, max_feedrate.e * steps_per_unit.e,
max_feedrate.x * steps_per_unit.x, max_feedrate.x * steps_per_unit.x,
max_feedrate.y * steps_per_unit.y, max_feedrate.y * steps_per_unit.y,
@ -330,98 +343,123 @@ constexpr ena_mask_t enable_overlap[] = {
max_feedrate.u * steps_per_unit.u, max_feedrate.u * steps_per_unit.u,
max_feedrate.v * steps_per_unit.v, max_feedrate.v * steps_per_unit.v,
max_feedrate.w * steps_per_unit.w 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; OPTARG(ADAPTIVE_STEP_SMOOTHING, MIN_STEP_ISR_FREQUENCY)
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; );
constexpr float max_step_rate = _MIN(max_isr_rate,
TERN0(INPUT_SHAPING_X, max_feedrate.x * steps_per_unit.x) +
TERN0(INPUT_SHAPING_Y, max_feedrate.y * steps_per_unit.y)
);
#else
constexpr float max_step_rate = TERN0(INPUT_SHAPING_X, max_feedrate.x * steps_per_unit.x) +
TERN0(INPUT_SHAPING_Y, max_feedrate.y * steps_per_unit.y);
#endif
constexpr uint16_t shaping_echoes = max_step_rate / _MIN(0x7FFFFFFFL OPTARG(INPUT_SHAPING_X, SHAPING_FREQ_X) OPTARG(INPUT_SHAPING_Y, SHAPING_FREQ_Y)) / 2 + 3;
class DelayTimeManager { typedef IF<ENABLED(__AVR__), uint16_t, uint32_t>::type shaping_time_t;
enum shaping_echo_t { ECHO_NONE = 0, ECHO_FWD = 1, ECHO_BWD = 2 };
struct shaping_echo_axis_t {
#if ENABLED(INPUT_SHAPING_X)
shaping_echo_t x:2;
#endif
#if ENABLED(INPUT_SHAPING_Y)
shaping_echo_t y:2;
#endif
};
class ShapingQueue {
private: private:
static shaping_time_t now; static shaping_time_t now;
#ifdef HAS_SHAPING_X static shaping_time_t times[shaping_echoes];
static shaping_time_t delay_x; 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 #endif
#ifdef HAS_SHAPING_Y #if ENABLED(INPUT_SHAPING_Y)
static shaping_time_t delay_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 #endif
public: public:
static void decrement_delays(const shaping_time_t interval) { now += interval; } 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);
}
static void set_delay(const AxisEnum axis, const shaping_time_t delay) { static void set_delay(const AxisEnum axis, const shaping_time_t delay) {
TERN_(HAS_SHAPING_X, if (axis == X_AXIS) delay_x = delay); TERN_(INPUT_SHAPING_X, if (axis == X_AXIS) delay_x = delay);
TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) delay_y = delay); TERN_(INPUT_SHAPING_Y, if (axis == Y_AXIS) delay_y = delay);
} }
}; static void enqueue(const bool x_step, const bool x_forward, const bool y_step, const bool y_forward) {
TERN_(INPUT_SHAPING_X, if (head_x == tail && x_step) peek_x_val = delay_x);
template<int SIZE> TERN_(INPUT_SHAPING_Y, if (head_y == tail && y_step) peek_y_val = delay_y);
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; times[tail] = now;
if (++tail == SIZE) tail = 0; TERN_(INPUT_SHAPING_X, echo_axes[tail].x = x_step ? (x_forward ? ECHO_FWD : ECHO_BWD) : ECHO_NONE);
TERN_(INPUT_SHAPING_Y, echo_axes[tail].y = y_step ? (y_forward ? ECHO_FWD : ECHO_BWD) : ECHO_NONE);
if (++tail == shaping_echoes) tail = 0;
TERN_(INPUT_SHAPING_X, _free_count_x--);
TERN_(INPUT_SHAPING_Y, _free_count_y--);
TERN_(INPUT_SHAPING_X, if (echo_axes[head_x].x == ECHO_NONE) dequeue_x());
TERN_(INPUT_SHAPING_Y, if (echo_axes[head_y].y == ECHO_NONE) dequeue_y());
} }
#ifdef HAS_SHAPING_X #if ENABLED(INPUT_SHAPING_X)
shaping_time_t peek_x() { static shaping_time_t peek_x() { return peek_x_val; }
if (head_x != tail) return times[head_x] + delay_x - now; static bool dequeue_x() {
else return shaping_time_t(-1); 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;
} }
void dequeue_x() { if (++head_x == SIZE) head_x = 0; } static bool empty_x() { return head_x == tail; }
bool empty_x() { return head_x == tail; } static uint16_t free_count_x() { return _free_count_x; }
uint16_t free_count_x() { return head_x > tail ? head_x - tail - 1 : head_x + SIZE - tail - 1; }
#endif #endif
#ifdef HAS_SHAPING_Y #if ENABLED(INPUT_SHAPING_Y)
shaping_time_t peek_y() { static shaping_time_t peek_y() { return peek_y_val; }
if (head_y != tail) return times[head_y] + delay_y - now; static bool dequeue_y() {
else return shaping_time_t(-1); 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;
} }
void dequeue_y() { if (++head_y == SIZE) head_y = 0; } static bool empty_y() { return head_y == tail; }
bool empty_y() { return head_y == tail; } static uint16_t free_count_y() { return _free_count_y; }
uint16_t free_count_y() { return head_y > tail ? head_y - tail - 1 : head_y + SIZE - tail - 1; }
#endif #endif
void purge() { auto temp = TERN_(HAS_SHAPING_X, head_x) = TERN_(HAS_SHAPING_Y, head_y) = tail; UNUSED(temp);} static void purge() {
}; const auto st = shaping_time_t(-1);
#if ENABLED(INPUT_SHAPING_X)
class ParamDelayQueue : public DelayQueue<shaping_segments> { head_x = tail; _free_count_x = shaping_echoes - 1; peek_x_val = st;
private:
#ifdef HAS_SHAPING_X
int32_t params_x[shaping_segments];
#endif #endif
#ifdef HAS_SHAPING_Y #if ENABLED(INPUT_SHAPING_Y)
int32_t params_y[shaping_segments]; head_y = tail; _free_count_y = shaping_echoes - 1; peek_y_val = st;
#endif #endif
public:
void enqueue(const int32_t param_x, const int32_t param_y) {
TERN(HAS_SHAPING_X, params_x[DelayQueue<shaping_segments>::tail] = param_x, UNUSED(param_x));
TERN(HAS_SHAPING_Y, params_y[DelayQueue<shaping_segments>::tail] = param_y, UNUSED(param_y));
DelayQueue<shaping_segments>::enqueue();
} }
#ifdef HAS_SHAPING_X
const int32_t dequeue_x() {
const int32_t result = params_x[DelayQueue<shaping_segments>::head_x];
DelayQueue<shaping_segments>::dequeue_x();
return result;
}
#endif
#ifdef HAS_SHAPING_Y
const int32_t dequeue_y() {
const int32_t result = params_y[DelayQueue<shaping_segments>::head_y];
DelayQueue<shaping_segments>::dequeue_y();
return result;
}
#endif
}; };
struct ShapeParams { struct ShapeParams {
float frequency; float frequency;
float zeta; float zeta;
uint8_t factor; bool enabled;
int32_t dividend; int16_t delta_error = 0; // delta_error for seconday bresenham mod 128
uint8_t factor1;
uint8_t factor2;
bool forward;
int32_t last_block_end_pos = 0;
}; };
#endif // INPUT_SHAPING #endif // HAS_SHAPING
// //
// Stepper class definition // Stepper class definition
@ -527,13 +565,11 @@ class Stepper {
static bool bezier_2nd_half; // If Bézier curve has been initialized or not static bool bezier_2nd_half; // If Bézier curve has been initialized or not
#endif #endif
#if ENABLED(INPUT_SHAPING) #if HAS_SHAPING
static ParamDelayQueue shaping_dividend_queue; #if ENABLED(INPUT_SHAPING_X)
static DelayQueue<shaping_dividends> shaping_queue;
#if HAS_SHAPING_X
static ShapeParams shaping_x; static ShapeParams shaping_x;
#endif #endif
#if HAS_SHAPING_Y #if ENABLED(INPUT_SHAPING_Y)
static ShapeParams shaping_y; static ShapeParams shaping_y;
#endif #endif
#endif #endif
@ -597,7 +633,7 @@ class Stepper {
// The stepper block processing ISR phase // The stepper block processing ISR phase
static uint32_t block_phase_isr(); static uint32_t block_phase_isr();
#if ENABLED(INPUT_SHAPING) #if HAS_SHAPING
static void shaping_isr(); static void shaping_isr();
#endif #endif
@ -620,6 +656,20 @@ class Stepper {
// Check if the given block is busy or not - Must not be called from ISR contexts // Check if the given block is busy or not - Must not be called from ISR contexts
static bool is_block_busy(const block_t * const block); static bool is_block_busy(const block_t * const block);
#if HAS_SHAPING
// Check whether the stepper is processing any input shaping echoes
static bool input_shaping_busy() {
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());
if (was_on) hal.isr_on();
return result;
}
#endif
// Get the position of a stepper, in steps // Get the position of a stepper, in steps
static int32_t position(const AxisEnum axis); static int32_t position(const AxisEnum axis);
@ -754,7 +804,7 @@ class Stepper {
set_directions(); set_directions();
} }
#if ENABLED(INPUT_SHAPING) #if HAS_SHAPING
static void set_shaping_damping_ratio(const AxisEnum axis, const float zeta); static void set_shaping_damping_ratio(const AxisEnum axis, const float zeta);
static float get_shaping_damping_ratio(const AxisEnum axis); static float get_shaping_damping_ratio(const AxisEnum axis);
static void set_shaping_frequency(const AxisEnum axis, const float freq); static void set_shaping_frequency(const AxisEnum axis, const float freq);

4
buildroot/tests/mega2560
View File

@ -80,9 +80,9 @@ 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 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 \ opt_enable MIXING_EXTRUDER GRADIENT_MIX GRADIENT_VTOOL CR10_STOCKDISPLAY \
USE_CONTROLLER_FAN CONTROLLER_FAN_EDITABLE CONTROLLER_FAN_IGNORE_Z \ USE_CONTROLLER_FAN CONTROLLER_FAN_EDITABLE CONTROLLER_FAN_IGNORE_Z \
FILAMENT_RUNOUT_SENSOR ADVANCED_PAUSE_FEATURE NOZZLE_PARK_FEATURE INPUT_SHAPING FILAMENT_RUNOUT_SENSOR ADVANCED_PAUSE_FEATURE NOZZLE_PARK_FEATURE INPUT_SHAPING_X INPUT_SHAPING_Y
opt_disable DISABLE_INACTIVE_EXTRUDER opt_disable DISABLE_INACTIVE_EXTRUDER
exec_test $1 $2 "Azteeg X3 | Mixing Extruder (x5) | Gradient Mix | Greek" "$3" exec_test $1 $2 "Azteeg X3 | Mixing Extruder (x5) | Gradient Mix | Input Shaping | Greek" "$3"
# #
# Test SPEAKER with BOARD_BQ_ZUM_MEGA_3D and BQ_LCD_SMART_CONTROLLER # Test SPEAKER with BOARD_BQ_ZUM_MEGA_3D and BQ_LCD_SMART_CONTROLLER

2
ini/features.ini
View File

@ -187,7 +187,7 @@ HAS_DUPLICATION_MODE = src_filter=+<src/gcode/control/M605.cpp
LIN_ADVANCE = src_filter=+<src/gcode/feature/advance> LIN_ADVANCE = src_filter=+<src/gcode/feature/advance>
PHOTO_GCODE = src_filter=+<src/gcode/feature/camera> PHOTO_GCODE = src_filter=+<src/gcode/feature/camera>
CONTROLLER_FAN_EDITABLE = src_filter=+<src/gcode/feature/controllerfan> CONTROLLER_FAN_EDITABLE = src_filter=+<src/gcode/feature/controllerfan>
INPUT_SHAPING = src_filter=+<src/gcode/feature/input_shaping> HAS_SHAPING = src_filter=+<src/gcode/feature/input_shaping>
GCODE_MACROS = src_filter=+<src/gcode/feature/macro> GCODE_MACROS = src_filter=+<src/gcode/feature/macro>
GRADIENT_MIX = src_filter=+<src/gcode/feature/mixing/M166.cpp> GRADIENT_MIX = src_filter=+<src/gcode/feature/mixing/M166.cpp>
HAS_SAVED_POSITIONS = src_filter=+<src/gcode/feature/pause/G60.cpp> +<src/gcode/feature/pause/G61.cpp> HAS_SAVED_POSITIONS = src_filter=+<src/gcode/feature/pause/G60.cpp> +<src/gcode/feature/pause/G61.cpp>