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

🚀 ZV Input Shaping (#24797)

Browse Source
This commit is contained in:
tombrazier 2022-10-21 22:34:22 +01:00 committed by Scott Lahteine
parent 5765449867
commit c1f0f26bff
15 changed files with 657 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
Marlin/Configuration_adv.h
View File

@ -1055,6 +1055,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<factor> F<frequency>:
*
* D<factor> Set the zeta/damping factor. If axes (X, Y, etc.) are not specified, set for all axes.
* F<frequency> 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

83
Marlin/src/gcode/feature/input_shaping/M593.cpp Normal file
View File

@ -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 <https://www.gnu.org/licenses/>.
*
*/
#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<factor> Set the zeta/damping factor. If axes (X, Y, etc.) are not specified, set for all axes.
* F<frequency> 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

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

@ -933,6 +933,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

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

@ -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<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)
* M605 - Set Dual X-Carriage movement mode: "M605 S<mode> [X<x_offset>] [R<temp_offset>]". (Requires DUAL_X_CARRIAGE)
@ -1080,6 +1081,11 @@ private:
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();

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

@ -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

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

@ -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

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

@ -399,6 +399,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");

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

@ -31,6 +31,7 @@
#include "menu_item.h"
#include "../../MarlinCore.h"
#include "../../module/planner.h"
#include "../../module/stepper.h"
#if DISABLED(NO_VOLUMETRICS)
#include "../../gcode/parser.h"
@ -80,8 +81,6 @@ void menu_backlash();
#if HAS_MOTOR_CURRENT_PWM
#include "../../module/stepper.h"
void menu_pwm() {
START_MENU();
BACK_ITEM(MSG_ADVANCED_SETTINGS);
@ -538,6 +537,39 @@ void menu_backlash();
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() {
@ -657,6 +689,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);

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

@ -2483,6 +2483,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;

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

@ -577,6 +577,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
} SettingsData;
//static_assert(sizeof(SettingsData) <= MARLIN_EEPROM_SIZE, "EEPROM too small to contain SettingsData!");
@ -1602,6 +1614,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
//
@ -2573,6 +2599,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
//
@ -3343,6 +3390,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)
@ -3590,6 +3651,11 @@ void MarlinSettings::reset() {
//
TERN_(HAS_STEALTHCHOP, gcode.M569_report(forReplay));
//
// Input Shaping
//
TERN_(INPUT_SHAPING, gcode.M593_report(forReplay));
//
// Linear Advance
//

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

@ -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<shaping_dividends> 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
*
@ -570,16 +592,6 @@ void Stepper::set_directions() {
DIR_WAIT_BEFORE();
#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; \
}
TERN_(HAS_X_DIR, SET_STEP_DIR(X)); // A
TERN_(HAS_Y_DIR, SET_STEP_DIR(Y)); // B
TERN_(HAS_Z_DIR, SET_STEP_DIR(Z)); // C
@ -1467,8 +1479,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();
@ -1497,10 +1521,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?
);
//
@ -1512,6 +1540,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
@ -1604,11 +1634,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;
@ -1627,6 +1665,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
@ -1641,6 +1682,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)]) { \
@ -1655,9 +1712,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) {
@ -1765,12 +1819,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);
@ -1855,7 +1919,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
@ -1895,12 +1959,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
@ -2365,12 +2479,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;
@ -2485,7 +2643,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
@ -2816,6 +2974,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
*
@ -3021,7 +3224,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

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

@ -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<ENABLED(__AVR__), uint16_t, uint32_t>::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<int SIZE>
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<shaping_segments> {
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<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 {
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_dividends> 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

2
buildroot/tests/mega2560
View File

@ -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"

1
ini/features.ini
View File

@ -187,6 +187,7 @@ HAS_DUPLICATION_MODE = src_filter=+<src/gcode/control/M605.cpp
LIN_ADVANCE = src_filter=+<src/gcode/feature/advance>
PHOTO_GCODE = src_filter=+<src/gcode/feature/camera>
CONTROLLER_FAN_EDITABLE = src_filter=+<src/gcode/feature/controllerfan>
INPUT_SHAPING = src_filter=+<src/gcode/feature/input_shaping>
GCODE_MACROS = src_filter=+<src/gcode/feature/macro>
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>

1
platformio.ini
View File

@ -192,6 +192,7 @@ default_src_filter = +<src/*> -<src/config> -<src/HAL> +<src/HAL/shared> -<src/t
-<src/gcode/feature/advance>
-<src/gcode/feature/camera>
-<src/gcode/feature/i2c>
-<src/gcode/feature/input_shaping>
-<src/gcode/feature/L6470>
-<src/gcode/feature/leds/M150.cpp>
-<src/gcode/feature/leds/M7219.cpp>