From 6dc7b09866922ccf6bfd9424a0015b2a253d4a8e Mon Sep 17 00:00:00 2001 From: steven2016gsc Date: Fri, 2 Aug 2024 21:02:49 -0400 Subject: [PATCH] 1st commit of FT migration for Marlin Added FT support for Marlin (bugfix-2.1.x). (20240802) Based on latest Marlin commit (https://github.com/MarlinFirmware/Marlin/commit/1662e953fcf9e1ee4c5005480c48ddc8b281ab61) --- Marlin/Configuration.h | 154 +++---- Marlin/Configuration_adv.h | 45 +- Marlin/src/gcode/feature/ft_motion/M493.cpp | 272 +++++------ Marlin/src/gcode/feature/ft_motion/M494.cpp | 211 +++++++++ Marlin/src/gcode/gcode.cpp | 1 + Marlin/src/gcode/gcode.h | 3 +- Marlin/src/module/endstops.cpp | 44 +- Marlin/src/module/ft_motion.cpp | 479 ++++++++++++-------- Marlin/src/module/ft_motion.h | 62 ++- Marlin/src/module/ft_types.h | 49 +- Marlin/src/module/planner.cpp | 29 +- Marlin/src/module/stepper.cpp | 252 +++++----- platformio.ini | 4 +- 13 files changed, 993 insertions(+), 612 deletions(-) create mode 100644 Marlin/src/gcode/feature/ft_motion/M494.cpp diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h index dc04fd2..aa79e2e 100644 --- a/Marlin/Configuration.h +++ b/Marlin/Configuration.h @@ -50,7 +50,7 @@ * * Calibration Guides: https://reprap.org/wiki/Calibration * https://reprap.org/wiki/Triffid_Hunter%27s_Calibration_Guide - * https://web.archive.org/web/20220907014303/sites.google.com/site/repraplogphase/calibration-of-your-reprap + * https://web.archive.org/web/20220907014303/https://sites.google.com/site/repraplogphase/calibration-of-your-reprap * https://youtu.be/wAL9d7FgInk * https://teachingtechyt.github.io/calibration.html * @@ -68,7 +68,7 @@ // Choose the name from boards.h that matches your setup #ifndef MOTHERBOARD - #define MOTHERBOARD BOARD_RAMPS_14_EFB + #define MOTHERBOARD BOARD_CREALITY_V4 #endif /** @@ -79,7 +79,7 @@ * * :[-1, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9] */ -#define SERIAL_PORT 0 +#define SERIAL_PORT 1 /** * Serial Port Baud Rate @@ -92,7 +92,7 @@ * * :[2400, 9600, 19200, 38400, 57600, 115200, 250000, 500000, 1000000] */ -#define BAUDRATE 250000 +#define BAUDRATE 115200 //#define BAUD_RATE_GCODE // Enable G-code M575 to set the baud rate @@ -126,7 +126,7 @@ //#define BLUETOOTH // Name displayed in the LCD "Ready" message and Info menu -//#define CUSTOM_MACHINE_NAME "3D Printer" +#define CUSTOM_MACHINE_NAME "Ender 3 V2 Neo - Ulendo FT" // Printer's unique ID, used by some programs to differentiate between machines. // Choose your own or use a service like https://www.uuidgenerator.net/version4 @@ -149,9 +149,9 @@ * TMC5160, TMC5160_STANDALONE * :['A4988', 'A5984', 'DRV8825', 'LV8729', 'TB6560', 'TB6600', 'TMC2100', 'TMC2130', 'TMC2130_STANDALONE', 'TMC2160', 'TMC2160_STANDALONE', 'TMC2208', 'TMC2208_STANDALONE', 'TMC2209', 'TMC2209_STANDALONE', 'TMC2660', 'TMC2660_STANDALONE', 'TMC5130', 'TMC5130_STANDALONE', 'TMC5160', 'TMC5160_STANDALONE'] */ -#define X_DRIVER_TYPE A4988 -#define Y_DRIVER_TYPE A4988 -#define Z_DRIVER_TYPE A4988 +#define X_DRIVER_TYPE TMC2208_STANDALONE +#define Y_DRIVER_TYPE TMC2208_STANDALONE +#define Z_DRIVER_TYPE TMC2208_STANDALONE //#define X2_DRIVER_TYPE A4988 //#define Y2_DRIVER_TYPE A4988 //#define Z2_DRIVER_TYPE A4988 @@ -163,7 +163,7 @@ //#define U_DRIVER_TYPE A4988 //#define V_DRIVER_TYPE A4988 //#define W_DRIVER_TYPE A4988 -#define E0_DRIVER_TYPE A4988 +#define E0_DRIVER_TYPE TMC2208_STANDALONE //#define E1_DRIVER_TYPE A4988 //#define E2_DRIVER_TYPE A4988 //#define E3_DRIVER_TYPE A4988 @@ -699,9 +699,9 @@ #define DEFAULT_Ki_LIST { 1.08, 1.08 } #define DEFAULT_Kd_LIST { 114.00, 114.00 } #else - #define DEFAULT_Kp 22.20 - #define DEFAULT_Ki 1.08 - #define DEFAULT_Kd 114.00 + #define DEFAULT_Kp 28.72 + #define DEFAULT_Ki 2.62 + #define DEFAULT_Kd 78.81 #endif #else #define BANG_MAX 255 // Limit hotend current while in bang-bang mode; 255=full current @@ -789,9 +789,9 @@ // 120V 250W silicone heater into 4mm borosilicate (MendelMax 1.5+) // from FOPDT model - kp=.39 Tp=405 Tdead=66, Tc set to 79.2, aggressive factor of .15 (vs .1, 1, 10) - #define DEFAULT_bedKp 10.00 - #define DEFAULT_bedKi .023 - #define DEFAULT_bedKd 305.4 + #define DEFAULT_bedKp 462.1 + #define DEFAULT_bedKi 85.47 + #define DEFAULT_bedKd 642.59 // FIND YOUR OWN: "M303 E-1 C8 S90" to run autotune on the bed at 90 degreesC for 8 cycles. #else @@ -894,7 +894,7 @@ #define THERMAL_PROTECTION_HOTENDS // Enable thermal protection for all extruders #define THERMAL_PROTECTION_BED // Enable thermal protection for the heated bed -#define THERMAL_PROTECTION_CHAMBER // Enable thermal protection for the heated chamber +//#define THERMAL_PROTECTION_CHAMBER // Enable thermal protection for the heated chamber #define THERMAL_PROTECTION_COOLER // Enable thermal protection for the laser cooling //=========================================================================== @@ -1202,7 +1202,7 @@ // Enable this feature if all enabled endstop pins are interrupt-capable. // This will remove the need to poll the interrupt pins, saving many CPU cycles. -//#define ENDSTOP_INTERRUPTS_FEATURE +#define ENDSTOP_INTERRUPTS_FEATURE /** * Endstop Noise Threshold @@ -1246,7 +1246,7 @@ * Override with M92 (when enabled below) * X, Y, Z [, I [, J [, K...]]], E0 [, E1[, E2...]] */ -#define DEFAULT_AXIS_STEPS_PER_UNIT { 80, 80, 400, 500 } +#define DEFAULT_AXIS_STEPS_PER_UNIT { 80, 80, 400, 93 } /** * Enable support for M92. Disable to save at least ~530 bytes of flash. @@ -1258,11 +1258,11 @@ * Override with M203 * X, Y, Z [, I [, J [, K...]]], E0 [, E1[, E2...]] */ -#define DEFAULT_MAX_FEEDRATE { 300, 300, 5, 25 } +#define DEFAULT_MAX_FEEDRATE { 1000, 1000, 5, 25 } //#define LIMITED_MAX_FR_EDITING // Limit edit via M203 or LCD to DEFAULT_MAX_FEEDRATE * 2 #if ENABLED(LIMITED_MAX_FR_EDITING) - #define MAX_FEEDRATE_EDIT_VALUES { 600, 600, 10, 50 } // ...or, set your own edit limits + #define MAX_FEEDRATE_EDIT_VALUES { 1000, 1000, 10, 50 } // ...or, set your own edit limits #endif /** @@ -1271,7 +1271,7 @@ * Override with M201 * X, Y, Z [, I [, J [, K...]]], E0 [, E1[, E2...]] */ -#define DEFAULT_MAX_ACCELERATION { 3000, 3000, 100, 10000 } +#define DEFAULT_MAX_ACCELERATION { 6000, 6000, 100, 10000 } //#define LIMITED_MAX_ACCEL_EDITING // Limit edit via M201 or LCD to DEFAULT_MAX_ACCELERATION * 2 #if ENABLED(LIMITED_MAX_ACCEL_EDITING) @@ -1298,7 +1298,7 @@ * When changing speed and direction, if the difference is less than the * value set here, it may happen instantaneously. */ -//#define CLASSIC_JERK +#define CLASSIC_JERK #if ENABLED(CLASSIC_JERK) #define DEFAULT_XJERK 10.0 #define DEFAULT_YJERK 10.0 @@ -1356,10 +1356,10 @@ * The probe replaces the Z-MIN endstop and is used for Z homing. * (Automatically enables USE_PROBE_FOR_Z_HOMING.) */ -#define Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN +//#define Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN // Force the use of the probe for Z-axis homing -//#define USE_PROBE_FOR_Z_HOMING +#define USE_PROBE_FOR_Z_HOMING /** * Z_MIN_PROBE_PIN @@ -1374,7 +1374,7 @@ * - Normally-closed (NC) also connect to GND. * - Normally-open (NO) also connect to 5V. */ -//#define Z_MIN_PROBE_PIN -1 +#define Z_MIN_PROBE_PIN PB1 /** * Probe Type @@ -1416,7 +1416,7 @@ /** * The BLTouch probe uses a Hall effect sensor and emulates a servo. */ -//#define BLTOUCH +#define BLTOUCH /** * MagLev V4 probe by MDD @@ -1591,7 +1591,7 @@ * | [-] | * O-- FRONT --+ */ -#define NOZZLE_TO_PROBE_OFFSET { 10, 10, 0 } +#define NOZZLE_TO_PROBE_OFFSET { -44, -9, 0 } // Enable and set to use a specific tool for probing. Disable to allow any tool. #define PROBING_TOOL 0 @@ -1601,7 +1601,7 @@ // Most probes should stay away from the edges of the bed, but // with NOZZLE_AS_PROBE this can be negative for a wider probing area. -#define PROBING_MARGIN 10 +#define PROBING_MARGIN 25 // X and Y axis travel speed (mm/min) between probes #define XY_PROBE_FEEDRATE (133*60) @@ -1723,8 +1723,6 @@ #define PROBING_BED_TEMP 50 #endif -// @section stepper drivers - // For Inverting Stepper Enable Pins (Active Low) use 0, Non Inverting (Active High) use 1 // :{ 0:'Low', 1:'High' } #define X_ENABLE_ON 0 @@ -1762,8 +1760,8 @@ // Invert the stepper direction. Change (or reverse the motor connector) if an axis goes the wrong way. #define INVERT_X_DIR false -#define INVERT_Y_DIR true -#define INVERT_Z_DIR false +#define INVERT_Y_DIR false +#define INVERT_Z_DIR true //#define INVERT_I_DIR false //#define INVERT_J_DIR false //#define INVERT_K_DIR false @@ -1833,8 +1831,8 @@ // @section geometry // The size of the printable area -#define X_BED_SIZE 200 -#define Y_BED_SIZE 200 +#define X_BED_SIZE 220 +#define Y_BED_SIZE 220 // Travel limits (linear=mm, rotational=°) after homing, corresponding to endstop positions. #define X_MIN_POS 0 @@ -1842,7 +1840,7 @@ #define Z_MIN_POS 0 #define X_MAX_POS X_BED_SIZE #define Y_MAX_POS Y_BED_SIZE -#define Z_MAX_POS 200 +#define Z_MAX_POS 250 //#define I_MIN_POS 0 //#define I_MAX_POS 50 //#define J_MIN_POS 0 @@ -1870,7 +1868,7 @@ #if ENABLED(MIN_SOFTWARE_ENDSTOPS) #define MIN_SOFTWARE_ENDSTOP_X #define MIN_SOFTWARE_ENDSTOP_Y - #define MIN_SOFTWARE_ENDSTOP_Z + //#define MIN_SOFTWARE_ENDSTOP_Z #define MIN_SOFTWARE_ENDSTOP_I #define MIN_SOFTWARE_ENDSTOP_J #define MIN_SOFTWARE_ENDSTOP_K @@ -1898,8 +1896,6 @@ #endif /** - * @section filament runout sensors - * * Filament Runout Sensors * Mechanical or opto endstops are used to check for the presence of filament. * @@ -2055,7 +2051,7 @@ */ //#define AUTO_BED_LEVELING_3POINT //#define AUTO_BED_LEVELING_LINEAR -//#define AUTO_BED_LEVELING_BILINEAR +#define AUTO_BED_LEVELING_BILINEAR //#define AUTO_BED_LEVELING_UBL //#define MESH_BED_LEVELING @@ -2070,7 +2066,7 @@ * these options to restore the prior leveling state or to always enable * leveling immediately after G28. */ -//#define RESTORE_LEVELING_AFTER_G28 +#define RESTORE_LEVELING_AFTER_G28 //#define ENABLE_LEVELING_AFTER_G28 /** @@ -2285,7 +2281,7 @@ * - Allows Z homing only when XY positions are known and trusted. * - If stepper drivers sleep, XY homing may be required again before Z homing. */ -//#define Z_SAFE_HOMING +#define Z_SAFE_HOMING #if ENABLED(Z_SAFE_HOMING) #define Z_SAFE_HOMING_X_POINT X_CENTER // (mm) X point for Z homing @@ -2294,7 +2290,7 @@ #endif // Homing speeds (linear=mm/min, rotational=°/min) -#define HOMING_FEEDRATE_MM_M { (50*60), (50*60), (4*60) } +#define HOMING_FEEDRATE_MM_M { (60*60), (60*60), (8*60) } // Validate that endstops are triggered on homing moves #define VALIDATE_HOMING_ENDSTOPS @@ -2372,9 +2368,9 @@ * M501 - Read settings from EEPROM. (i.e., Throw away unsaved changes) * M502 - Revert settings to "factory" defaults. (Follow with M500 to init the EEPROM.) */ -//#define EEPROM_SETTINGS // Persistent storage with M500 and M501 +#define EEPROM_SETTINGS // Persistent storage with M500 and M501 //#define DISABLE_M503 // Saves ~2700 bytes of flash. Disable for release! -#define EEPROM_CHITCHAT // Give feedback on EEPROM commands. Disable to save flash. +//#define EEPROM_CHITCHAT // Give feedback on EEPROM commands. Disable to save flash. #define EEPROM_BOOT_SILENT // Keep M503 quiet and only give errors during first load #if ENABLED(EEPROM_SETTINGS) //#define EEPROM_AUTO_INIT // Init EEPROM automatically on any errors. @@ -2422,9 +2418,9 @@ #define PREHEAT_2_TEMP_CHAMBER 35 #define PREHEAT_2_FAN_SPEED 0 // Value from 0 to 255 +// @section motion + /** - * @section nozzle park - * * Nozzle Park * * Park the nozzle at the given XYZ position on idle or G27. @@ -2435,7 +2431,7 @@ * P1 Raise the nozzle always to Z-park height. * P2 Raise the nozzle by Z-park amount, limited to Z_MAX_POS. */ -//#define NOZZLE_PARK_FEATURE +#define NOZZLE_PARK_FEATURE #if ENABLED(NOZZLE_PARK_FEATURE) // Specify a park position as { X, Y, Z_raise } @@ -2447,8 +2443,6 @@ #endif /** - * @section nozzle clean - * * Clean Nozzle Feature * * Adds the G12 command to perform a nozzle cleaning process. @@ -2609,24 +2603,9 @@ //#include "Configuration_Secure.h" // External file with PASSWORD_DEFAULT_VALUE #endif -// @section media - -/** - * SD CARD - * - * SD Card support is disabled by default. If your controller has an SD slot, - * you must uncomment the following option or it won't work. - */ -//#define SDSUPPORT - -/** - * SD CARD: ENABLE CRC - * - * Use CRC checks and retries on the SD communication. - */ -#if ENABLED(SDSUPPORT) - //#define SD_CHECK_AND_RETRY -#endif +//============================================================================= +//============================= LCD and SD support ============================ +//============================================================================= // @section interface @@ -2673,6 +2652,21 @@ */ #define LCD_INFO_SCREEN_STYLE 0 +/** + * SD CARD + * + * SD Card support is disabled by default. If your controller has an SD slot, + * you must uncomment the following option or it won't work. + */ +#define SDSUPPORT + +/** + * SD CARD: ENABLE CRC + * + * Use CRC checks and retries on the SD communication. + */ +//#define SD_CHECK_AND_RETRY + /** * LCD Menu Items * @@ -2688,13 +2682,13 @@ // This option overrides the default number of encoder pulses needed to // produce one step. Should be increased for high-resolution encoders. // -//#define ENCODER_PULSES_PER_STEP 4 +#define ENCODER_PULSES_PER_STEP 4 // // Use this option to override the number of step signals required to // move between next/prev menu items. // -//#define ENCODER_STEPS_PER_MENU_ITEM 1 +#define ENCODER_STEPS_PER_MENU_ITEM 1 /** * Encoder Direction Options @@ -2762,8 +2756,8 @@ // Note: Test audio output with the G-Code: // M300 S P // -//#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 2 -//#define LCD_FEEDBACK_FREQUENCY_HZ 5000 +#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 2 +#define LCD_FEEDBACK_FREQUENCY_HZ 5000 // // Tone queue size, used to keep beeps from blocking execution. @@ -2801,7 +2795,7 @@ // // Original RADDS LCD Display+Encoder+SDCardReader -// https://web.archive.org/web/20200719145306/doku.radds.org/dokumentation/lcd-display/ +// https://web.archive.org/web/20200719145306/http://doku.radds.org/dokumentation/lcd-display/ // //#define RADDS_DISPLAY @@ -2867,7 +2861,7 @@ // // Elefu RA Board Control Panel -// https://web.archive.org/web/20140823033947/www.elefu.com/index.php?route=product/product&product_id=53 +// https://web.archive.org/web/20140823033947/http://www.elefu.com/index.php?route=product/product&product_id=53 // //#define RA_CONTROL_PANEL @@ -2999,7 +2993,7 @@ // // Cartesio UI -// https://web.archive.org/web/20180605050442/mauk.cc/webshop/cartesio-shop/electronics/user-interface +// https://web.archive.org/web/20180605050442/http://mauk.cc/webshop/cartesio-shop/electronics/user-interface // //#define CARTESIO_UI @@ -3417,7 +3411,7 @@ // // Ender-3 v2 OEM display. A DWIN display with Rotary Encoder. // -//#define DWIN_CREALITY_LCD // Creality UI +#define DWIN_CREALITY_LCD // Creality UI //#define DWIN_LCD_PROUI // Pro UI by MRiscoC //#define DWIN_CREALITY_LCD_JYERSUI // Jyers UI by Jacob Myers //#define DWIN_MARLINUI_PORTRAIT // MarlinUI (portrait orientation) @@ -3477,7 +3471,7 @@ // Use software PWM to drive the fan, as for the heaters. This uses a very low frequency // which is not as annoying as with the hardware PWM. On the other hand, if this frequency // is too low, you should also increment SOFT_PWM_SCALE. -//#define FAN_SOFT_PWM +#define FAN_SOFT_PWM // Incrementing this by 1 will double the software PWM frequency, // affecting heaters, and the fan if FAN_SOFT_PWM is enabled. @@ -3555,14 +3549,14 @@ #endif // Support for Adafruit NeoPixel LED driver -//#define NEOPIXEL_LED +#define NEOPIXEL_LED #if ENABLED(NEOPIXEL_LED) - #define NEOPIXEL_TYPE NEO_GRBW // NEO_GRBW, NEO_RGBW, NEO_GRB, NEO_RBG, etc. + #define NEOPIXEL_TYPE NEO_GRB // NEO_GRBW, NEO_RGBW, NEO_GRB, NEO_RBG, etc. // See https://github.com/adafruit/Adafruit_NeoPixel/blob/master/Adafruit_NeoPixel.h - //#define NEOPIXEL_PIN 4 // LED driving pin + #define NEOPIXEL_PIN PB2 // LED driving pin //#define NEOPIXEL2_TYPE NEOPIXEL_TYPE //#define NEOPIXEL2_PIN 5 - #define NEOPIXEL_PIXELS 30 // Number of LEDs in the strip. (Longest strip when NEOPIXEL2_SEPARATE is disabled.) + #define NEOPIXEL_PIXELS 4 // Number of LEDs in the strip. (Longest strip when NEOPIXEL2_SEPARATE is disabled.) #define NEOPIXEL_IS_SEQUENTIAL // Sequential display for temperature change - LED by LED. Disable to change all LEDs at once. #define NEOPIXEL_BRIGHTNESS 127 // Initial brightness (0-255) //#define NEOPIXEL_STARTUP_TEST // Cycle through colors at startup diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index f02f06e..8e84ea0 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1120,21 +1120,26 @@ * Enable/disable and set parameters with G-code M493. * See ft_types.h for named values used by FTM options. */ -//#define FT_MOTION +#define FT_MOTION #if ENABLED(FT_MOTION) //#define FTM_IS_DEFAULT_MOTION // Use FT Motion as the factory default? - #define FTM_DEFAULT_DYNFREQ_MODE dynFreqMode_DISABLED // Default mode of dynamic frequency calculation. (DISABLED, Z_BASED, MASS_BASED) - #define FTM_DEFAULT_SHAPER_X ftMotionShaper_NONE // Default shaper mode on X axis (NONE, ZV, ZVD, ZVDD, ZVDDD, EI, 2HEI, 3HEI, MZV) - #define FTM_DEFAULT_SHAPER_Y ftMotionShaper_NONE // Default shaper mode on Y axis + // FBS-related + #define FTM_DEFAULT_MODE ftMotionMode_ENABLED // Default mode of fixed time control. (Enums in ft_types.h) + #define FTM_DEFAULT_X_COMPENSATOR ftMotionCmpnstr_NONE // Default compensation / shaper mode on X axis + #define FTM_DEFAULT_Y_COMPENSATOR ftMotionCmpnstr_NONE // Default compensation / shaper mode on Y axis + // --- + #define FTM_DEFAULT_DYNFREQ_MODE dynFreqMode_DISABLED // Default mode of dynamic frequency calculation. (DISABLED, Z_BASED, MASS_BASED) + #define FTM_DEFAULT_SHAPER_X ftMotionShaper_NONE // Default shaper mode on X axis (NONE, ZV, ZVD, ZVDD, ZVDDD, EI, 2HEI, 3HEI, MZV) + #define FTM_DEFAULT_SHAPER_Y ftMotionShaper_NONE // Default shaper mode on Y axis #define FTM_SHAPING_DEFAULT_X_FREQ 37.0f // (Hz) Default peak frequency used by input shapers #define FTM_SHAPING_DEFAULT_Y_FREQ 37.0f // (Hz) Default peak frequency used by input shapers #define FTM_LINEAR_ADV_DEFAULT_ENA false // Default linear advance enable (true) or disable (false) - #define FTM_LINEAR_ADV_DEFAULT_K 0 // Default linear advance gain, integer value. (Acceleration-based scaling factor.) - #define FTM_SHAPING_ZETA_X 0.1f // Zeta used by input shapers for X axis - #define FTM_SHAPING_ZETA_Y 0.1f // Zeta used by input shapers for Y axis + #define FTM_LINEAR_ADV_DEFAULT_K 0.0f // Default linear advance gain + #define FTM_SHAPING_DEFAULT_ZETA_X 0.1f // Zeta used by input shapers for X axis + #define FTM_SHAPING_DEFAULT_ZETA_Y 0.1f // Zeta used by input shapers for Y axis - #define FTM_SHAPING_V_TOL_X 0.05f // Vibration tolerance used by EI input shapers for X axis - #define FTM_SHAPING_V_TOL_Y 0.05f // Vibration tolerance used by EI input shapers for Y axis + #define FTM_SHAPING_DEFAULT_V_TOL_X 0.05f // Vibration tolerance used by EI input shapers for X axis + #define FTM_SHAPING_DEFAULT_V_TOL_Y 0.05f // Vibration tolerance used by EI input shapers for Y axis //#define FT_MOTION_MENU // Provide a MarlinUI menu to set M493 parameters @@ -1152,6 +1157,10 @@ #define FTM_FS 1000 // (Hz) Frequency for trajectory generation. (Reciprocal of FTM_TS) #define FTM_TS 0.001f // (s) Time step for trajectory generation. (Reciprocal of FTM_FS) + // FBS-related: These values may be configured to adjust the duration of loop(). + #define FTM_STEPS_PER_LOOP 60 // Number of stepper commands to generate each loop() + #define FTM_POINTS_PER_LOOP 100 // Number of trajectory points to generate each loop() + #if DISABLED(COREXY) #define FTM_STEPPER_FS 20000 // (Hz) Frequency for stepper I/O update @@ -1192,12 +1201,18 @@ * If the buffer is too small at runtime, input shaping will have reduced * effectiveness during high speed movements. * - * Tune with M593 D F + * Tune with M593 D F: + * + * D Set the zeta/damping factor. If axes (X, Y, etc.) are not specified, set for all axes. + * F Set the frequency. If axes (X, Y, etc.) are not specified, set for all axes. + * T[map] Input Shaping type, 0:ZV, 1:EI, 2:2H EI (not implemented yet) + * X<1> Set the given parameters only for the X axis. + * Y<1> Set the given parameters only for the Y axis. */ //#define INPUT_SHAPING_X //#define INPUT_SHAPING_Y //#define INPUT_SHAPING_Z -#if ANY(INPUT_SHAPING_X, INPUT_SHAPING_Y, INPUT_SHAPING_Z) +#if ANY(INPUT_SHAPING_X, INPUT_SHAPING_Y) //, INPUT_SHAPING_Z #if ENABLED(INPUT_SHAPING_X) #define SHAPING_FREQ_X 40.0 // (Hz) The default dominant resonant frequency on the X axis. #define SHAPING_ZETA_X 0.15 // Damping ratio of the X axis (range: 0.0 = no damping to 1.0 = critical damping). @@ -1206,10 +1221,12 @@ #define SHAPING_FREQ_Y 40.0 // (Hz) The default dominant resonant frequency on the Y axis. #define SHAPING_ZETA_Y 0.15 // Damping ratio of the Y axis (range: 0.0 = no damping to 1.0 = critical damping). #endif + /* #if ENABLED(INPUT_SHAPING_Z) #define SHAPING_FREQ_Z 40.0 // (Hz) The default dominant resonant frequency on the Z axis. #define SHAPING_ZETA_Z 0.15 // Damping ratio of the Z axis (range: 0.0 = no damping to 1.0 = critical damping). #endif + */ //#define SHAPING_MIN_FREQ 20.0 // (Hz) By default the minimum of the shaping frequencies. Override to affect SRAM usage. //#define SHAPING_MAX_STEPRATE 10000 // By default the maximum total step rate of the shaped axes. Override to affect SRAM usage. //#define SHAPING_MENU // Add a menu to the LCD to set shaping parameters. @@ -1331,6 +1348,8 @@ //#define CALIBRATION_SCRIPT_PRE "M117 Starting Auto-Calibration\nT0\nG28\nG12\nM117 Calibrating..." //#define CALIBRATION_SCRIPT_POST "M500\nM117 Calibration data saved" + #define CALIBRATION_MEASUREMENT_RESOLUTION 0.01 // mm + #define CALIBRATION_FEEDRATE_SLOW 60 // mm/min #define CALIBRATION_FEEDRATE_FAST 1200 // mm/min #define CALIBRATION_FEEDRATE_TRAVEL 3000 // mm/min @@ -3443,7 +3462,7 @@ /** * Step on both rising and falling edge signals (as with a square wave). */ - #define EDGE_STEPPING + //#define EDGE_STEPPING /** * Enable M122 debugging command for TMC stepper drivers. @@ -3525,7 +3544,7 @@ //#define PHOTOGRAPH_PIN 23 // Canon Hack Development Kit - // https://web.archive.org/web/20200920094805/captain-slow.dk/2014/03/09/3d-printing-timelapses/ + // https://web.archive.org/web/20200920094805/https://captain-slow.dk/2014/03/09/3d-printing-timelapses/ //#define CHDK_PIN 4 // Optional second move with delay to trigger the camera shutter diff --git a/Marlin/src/gcode/feature/ft_motion/M493.cpp b/Marlin/src/gcode/feature/ft_motion/M493.cpp index 993b4b2..c679119 100644 --- a/Marlin/src/gcode/feature/ft_motion/M493.cpp +++ b/Marlin/src/gcode/feature/ft_motion/M493.cpp @@ -28,6 +28,7 @@ #include "../../../module/ft_motion.h" #include "../../../module/stepper.h" +/* void say_shaper_type(const AxisEnum a) { SERIAL_ECHOPGM(" axis "); switch (ftMotion.cfg.shaper[a]) { @@ -54,66 +55,67 @@ void say_shaper_type(const AxisEnum a) { #else #define AXIS_1_NAME "Y" #endif +*/ + +#define CMPNSTR_IS_SHAPER(A) WITHIN(ftMotion.cfg.cmpnstr[A], ftMotionCmpnstr_ZV, ftMotionCmpnstr_MZV) +#define CMPNSTR_IS_EISHAPER(A) WITHIN(ftMotion.cfg.cmpnstr[A], ftMotionCmpnstr_EI, ftMotionCmpnstr_3HEI) + void say_shaping() { // FT Enabled - SERIAL_ECHO_TERNARY(ftMotion.cfg.active, "Fixed-Time Motion ", "en", "dis", "abled"); + SERIAL_ECHO_TERNARY(ftMotion.cfg.mode, "Fixed-Time Motion ", "en", "dis", "abled"); // FT Shaping #if HAS_X_AXIS - if (CMPNSTR_HAS_SHAPER(X_AXIS)) { - SERIAL_ECHOPGM(" with " AXIS_0_NAME); - say_shaper_type(X_AXIS); - } - #endif - #if HAS_Y_AXIS - if (CMPNSTR_HAS_SHAPER(Y_AXIS)) { - SERIAL_ECHOPGM(" and with " AXIS_1_NAME); - say_shaper_type(Y_AXIS); + if (CMPNSTR_IS_SHAPER(X_AXIS)) { + SERIAL_ECHOPGM(" with X/A axis "); + switch (ftMotion.cfg.cmpnstr[X_AXIS]) { + default: break; + case ftMotionCmpnstr_ZV: SERIAL_ECHOPGM("ZV"); break; + case ftMotionCmpnstr_ZVD: SERIAL_ECHOPGM("ZVD"); break; + case ftMotionCmpnstr_ZVDD: SERIAL_ECHOPGM("ZVDD"); break; + case ftMotionCmpnstr_ZVDDD: SERIAL_ECHOPGM("ZVDDD"); break; + case ftMotionCmpnstr_EI: SERIAL_ECHOPGM("EI"); break; + case ftMotionCmpnstr_2HEI: SERIAL_ECHOPGM("2 Hump EI"); break; + case ftMotionCmpnstr_3HEI: SERIAL_ECHOPGM("3 Hump EI"); break; + case ftMotionCmpnstr_MZV: SERIAL_ECHOPGM("MZV"); break; + } + SERIAL_ECHOPGM(" shaping"); } - #endif - - SERIAL_ECHOLNPGM("."); - - const bool z_based = TERN0(HAS_DYNAMIC_FREQ_MM, ftMotion.cfg.dynFreqMode == dynFreqMode_Z_BASED), - g_based = TERN0(HAS_DYNAMIC_FREQ_G, ftMotion.cfg.dynFreqMode == dynFreqMode_MASS_BASED), - dynamic = z_based || g_based; - - // FT Dynamic Frequency Mode - if (CMPNSTR_HAS_SHAPER(X_AXIS) || CMPNSTR_HAS_SHAPER(Y_AXIS)) { - #if HAS_DYNAMIC_FREQ - SERIAL_ECHOPGM("Dynamic Frequency Mode "); - switch (ftMotion.cfg.dynFreqMode) { - default: - case dynFreqMode_DISABLED: SERIAL_ECHOPGM("disabled"); break; - #if HAS_DYNAMIC_FREQ_MM - case dynFreqMode_Z_BASED: SERIAL_ECHOPGM("Z-based"); break; - #endif - #if HAS_DYNAMIC_FREQ_G - case dynFreqMode_MASS_BASED: SERIAL_ECHOPGM("Mass-based"); break; - #endif + #if HAS_Y_AXIS + if (CMPNSTR_IS_SHAPER(Y_AXIS)) { + SERIAL_ECHOPGM(" and with Y/B axis "); + switch (ftMotion.cfg.cmpnstr[Y_AXIS]) { + default: break; + case ftMotionCmpnstr_ZV: SERIAL_ECHOPGM("ZV"); break; + case ftMotionCmpnstr_ZVD: SERIAL_ECHOPGM("ZVD"); break; + case ftMotionCmpnstr_ZVDD: SERIAL_ECHOPGM("ZVDD"); break; + case ftMotionCmpnstr_ZVDDD: SERIAL_ECHOPGM("ZVDDD"); break; + case ftMotionCmpnstr_EI: SERIAL_ECHOPGM("EI"); break; + case ftMotionCmpnstr_2HEI: SERIAL_ECHOPGM("2 Hump EI"); break; + case ftMotionCmpnstr_3HEI: SERIAL_ECHOPGM("3 Hump EI"); break; + case ftMotionCmpnstr_MZV: SERIAL_ECHOPGM("MZV"); break; + } + SERIAL_ECHOPGM(" shaping"); } - SERIAL_ECHOLNPGM("."); #endif + #endif + SERIAL_ECHOLNPGM("."); - #if HAS_X_AXIS - SERIAL_ECHO_TERNARY(dynamic, AXIS_0_NAME " ", "base dynamic", "static", " shaper frequency: "); + #if HAS_X_AXIS + if (CMPNSTR_IS_SHAPER(X_AXIS)) { + SERIAL_ECHO( "X/A static compensator frequency: "); SERIAL_ECHO(p_float_t(ftMotion.cfg.baseFreq[X_AXIS], 2), F("Hz")); - #if HAS_DYNAMIC_FREQ - if (dynamic) SERIAL_ECHO(F(" scaling: "), p_float_t(ftMotion.cfg.dynFreqK[X_AXIS], 2), F("Hz/"), z_based ? F("mm") : F("g")); - #endif SERIAL_EOL(); - #endif - + } #if HAS_Y_AXIS - SERIAL_ECHO_TERNARY(dynamic, AXIS_1_NAME " ", "base dynamic", "static", " shaper frequency: "); - SERIAL_ECHO(p_float_t(ftMotion.cfg.baseFreq[Y_AXIS], 2), F(" Hz")); - #if HAS_DYNAMIC_FREQ - if (dynamic) SERIAL_ECHO(F(" scaling: "), p_float_t(ftMotion.cfg.dynFreqK[Y_AXIS], 2), F("Hz/"), z_based ? F("mm") : F("g")); - #endif - SERIAL_EOL(); + if (CMPNSTR_IS_SHAPER(Y_AXIS)) { + SERIAL_ECHO( "Y/B static compensator frequency: "); + SERIAL_ECHO(p_float_t(ftMotion.cfg.baseFreq[Y_AXIS], 2), F("Hz")); + SERIAL_EOL(); + } #endif - } + #endif #if HAS_EXTRUDERS SERIAL_ECHO_TERNARY(ftMotion.cfg.linearAdvEna, "Linear Advance ", "en", "dis", "abled"); @@ -129,22 +131,13 @@ void GcodeSuite::M493_report(const bool forReplay/*=true*/) { report_heading_etc(forReplay, F(STR_FT_MOTION)); const ft_config_t &c = ftMotion.cfg; - SERIAL_ECHOPGM(" M493 S", c.active); + SERIAL_ECHOPGM(" M493 S", c.mode); #if HAS_X_AXIS SERIAL_ECHOPGM(" A", c.baseFreq[X_AXIS]); #if HAS_Y_AXIS SERIAL_ECHOPGM(" B", c.baseFreq[Y_AXIS]); #endif #endif - #if HAS_DYNAMIC_FREQ - SERIAL_ECHOPGM(" D", c.dynFreqMode); - #if HAS_X_AXIS - SERIAL_ECHOPGM(" F", c.dynFreqK[X_AXIS]); - #if HAS_Y_AXIS - SERIAL_ECHOPGM(" H", c.dynFreqK[Y_AXIS]); - #endif - #endif - #endif #if HAS_EXTRUDERS SERIAL_ECHOPGM(" P", c.linearAdvEna, " K", c.linearAdvK); #endif @@ -190,55 +183,80 @@ void GcodeSuite::M493_report(const bool forReplay/*=true*/) { * R 0.00 Set the vibration tolerance for the Y axis */ void GcodeSuite::M493() { - struct { bool update:1, reset_ft:1, report_h:1; } flag = { false }; + struct { bool update_shpr_params:1, reset_ft:1, report_h:1; } flag = { false }; if (!parser.seen_any()) flag.report_h = true; // Parse 'S' mode parameter. - if (parser.seen('S')) { - const bool active = parser.value_bool(); - - if (active != ftMotion.cfg.active) { - switch (active) { - case false: flag.reset_ft = true; - case true: flag.report_h = true; - ftMotion.cfg.active = active; + if (parser.seenval('S')) { + const ftMotionMode_t newmm = (ftMotionMode_t)parser.value_byte(); + + if (newmm != ftMotion.cfg.mode) { + switch (newmm) { + default: SERIAL_ECHOLNPGM("?Invalid control mode [S] value."); return; + case ftMotionMode_DISABLED: flag.reset_ft = true; + case ftMotionMode_ENABLED: + ftMotion.cfg.mode = newmm; + flag.report_h = true; break; } } } + #if HAS_X_AXIS - auto set_shaper = [&](const AxisEnum axis, const char c) { - const ftMotionShaper_t newsh = (ftMotionShaper_t)parser.value_byte(); - if (newsh != ftMotion.cfg.shaper[axis]) { - switch (newsh) { - default: SERIAL_ECHOLNPGM("?Invalid [", C(c), "] shaper."); return true; - case ftMotionShaper_NONE: - case ftMotionShaper_ZV: - case ftMotionShaper_ZVD: - case ftMotionShaper_ZVDD: - case ftMotionShaper_ZVDDD: - case ftMotionShaper_EI: - case ftMotionShaper_2HEI: - case ftMotionShaper_3HEI: - case ftMotionShaper_MZV: - ftMotion.cfg.shaper[axis] = newsh; - flag.update = flag.report_h = true; + // Parse 'X' mode parameter. + if (parser.seenval('X')) { + const ftMotionCmpnstr_t newmm = (ftMotionCmpnstr_t)parser.value_byte(); + + if (newmm != ftMotion.cfg.cmpnstr[X_AXIS]) { + switch (newmm) { + default: SERIAL_ECHOLNPGM("?Invalid compensator / shaper [X] value."); return; + case ftMotionCmpnstr_NONE: + case ftMotionCmpnstr_ZV: + case ftMotionCmpnstr_ZVD: + case ftMotionCmpnstr_ZVDD: + case ftMotionCmpnstr_ZVDDD: + case ftMotionCmpnstr_EI: + case ftMotionCmpnstr_2HEI: + case ftMotionCmpnstr_3HEI: + case ftMotionCmpnstr_MZV: + flag.update_shpr_params = true; + ftMotion.cfg.cmpnstr[X_AXIS] = newmm; + flag.report_h = true; break; } } - return false; - }; - - if (parser.seenval('X') && set_shaper(X_AXIS, 'X')) return; // Parse 'X' mode parameter + } #if HAS_Y_AXIS - if (parser.seenval('Y') && set_shaper(Y_AXIS, 'Y')) return; // Parse 'Y' mode parameter + // Parse 'Y' mode parameter. + if (parser.seenval('Y')) { + const ftMotionCmpnstr_t newmm = (ftMotionCmpnstr_t)parser.value_byte(); + + if (newmm != ftMotion.cfg.cmpnstr[Y_AXIS]) { + switch (newmm) { + default: SERIAL_ECHOLNPGM("?Invalid compensator / shaper [Y] value."); return; + case ftMotionCmpnstr_NONE: + case ftMotionCmpnstr_ZV: + case ftMotionCmpnstr_ZVD: + case ftMotionCmpnstr_ZVDD: + case ftMotionCmpnstr_ZVDDD: + case ftMotionCmpnstr_EI: + case ftMotionCmpnstr_2HEI: + case ftMotionCmpnstr_3HEI: + case ftMotionCmpnstr_MZV: + flag.update_shpr_params = true; + ftMotion.cfg.cmpnstr[Y_AXIS] = newmm; + flag.report_h = true; + break; + } + } + } #endif + #endif - #endif // HAS_X_AXIS #if HAS_EXTRUDERS @@ -263,50 +281,16 @@ void GcodeSuite::M493() { #endif // HAS_EXTRUDERS - #if HAS_DYNAMIC_FREQ - - // Dynamic frequency mode parameter. - if (parser.seenval('D')) { - if (CMPNSTR_HAS_SHAPER(X_AXIS) || CMPNSTR_HAS_SHAPER(Y_AXIS)) { - const dynFreqMode_t val = dynFreqMode_t(parser.value_byte()); - switch (val) { - #if HAS_DYNAMIC_FREQ_MM - case dynFreqMode_Z_BASED: - #endif - #if HAS_DYNAMIC_FREQ_G - case dynFreqMode_MASS_BASED: - #endif - case dynFreqMode_DISABLED: - ftMotion.cfg.dynFreqMode = val; - flag.report_h = true; - break; - default: - SERIAL_ECHOLNPGM("?Invalid Dynamic Frequency Mode [D] value."); - break; - } - } - else { - SERIAL_ECHOLNPGM("?Wrong shaper for [D] Dynamic Frequency mode."); - } - } - - const bool modeUsesDynFreq = ( - TERN0(HAS_DYNAMIC_FREQ_MM, ftMotion.cfg.dynFreqMode == dynFreqMode_Z_BASED) - || TERN0(HAS_DYNAMIC_FREQ_G, ftMotion.cfg.dynFreqMode == dynFreqMode_MASS_BASED) - ); - - #endif // HAS_DYNAMIC_FREQ - #if HAS_X_AXIS // Parse frequency parameter (X axis). if (parser.seenval('A')) { - if (CMPNSTR_HAS_SHAPER(X_AXIS)) { + if (CMPNSTR_IS_SHAPER(X_AXIS)) { const float val = parser.value_float(); // TODO: Frequency minimum is dependent on the shaper used; the above check isn't always correct. if (WITHIN(val, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2)) { ftMotion.cfg.baseFreq[X_AXIS] = val; - flag.update = flag.reset_ft = flag.report_h = true; + flag.update_shpr_params = flag.reset_ft = flag.report_h = true; } else // Frequency out of range. SERIAL_ECHOLNPGM("Invalid [", C('A'), "] frequency value."); @@ -315,25 +299,13 @@ void GcodeSuite::M493() { SERIAL_ECHOLNPGM("Wrong mode for [", C('A'), "] frequency."); } - #if HAS_DYNAMIC_FREQ - // Parse frequency scaling parameter (X axis). - if (parser.seenval('F')) { - if (modeUsesDynFreq) { - ftMotion.cfg.dynFreqK[X_AXIS] = parser.value_float(); - flag.report_h = true; - } - else - SERIAL_ECHOLNPGM("Wrong mode for [", C('F'), "] frequency scaling."); - } - #endif - // Parse zeta parameter (X axis). if (parser.seenval('I')) { const float val = parser.value_float(); - if (CMPNSTR_HAS_SHAPER(X_AXIS)) { + if (CMPNSTR_IS_SHAPER(X_AXIS)) { if (WITHIN(val, 0.01f, 1.0f)) { ftMotion.cfg.zeta[0] = val; - flag.update = true; + flag.update_shpr_params = true; } else SERIAL_ECHOLNPGM("Invalid X zeta [", C('I'), "] value."); // Zeta out of range. @@ -348,7 +320,7 @@ void GcodeSuite::M493() { if (CMPNSTR_IS_EISHAPER(X_AXIS)) { if (WITHIN(val, 0.00f, 1.0f)) { ftMotion.cfg.vtol[0] = val; - flag.update = true; + flag.update_shpr_params = true; } else SERIAL_ECHOLNPGM("Invalid X vtol [", C('Q'), "] value."); // VTol out of range. @@ -363,11 +335,11 @@ void GcodeSuite::M493() { // Parse frequency parameter (Y axis). if (parser.seenval('B')) { - if (CMPNSTR_HAS_SHAPER(Y_AXIS)) { + if (CMPNSTR_IS_SHAPER(Y_AXIS)) { const float val = parser.value_float(); if (WITHIN(val, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2)) { ftMotion.cfg.baseFreq[Y_AXIS] = val; - flag.update = flag.reset_ft = flag.report_h = true; + flag.update_shpr_params = flag.reset_ft = flag.report_h = true; } else // Frequency out of range. SERIAL_ECHOLNPGM("Invalid frequency [", C('B'), "] value."); @@ -376,25 +348,13 @@ void GcodeSuite::M493() { SERIAL_ECHOLNPGM("Wrong mode for [", C('B'), "] frequency."); } - #if HAS_DYNAMIC_FREQ - // Parse frequency scaling parameter (Y axis). - if (parser.seenval('H')) { - if (modeUsesDynFreq) { - ftMotion.cfg.dynFreqK[Y_AXIS] = parser.value_float(); - flag.report_h = true; - } - else - SERIAL_ECHOLNPGM("Wrong mode for [", C('H'), "] frequency scaling."); - } - #endif - // Parse zeta parameter (Y axis). if (parser.seenval('J')) { const float val = parser.value_float(); - if (CMPNSTR_HAS_SHAPER(Y_AXIS)) { + if (CMPNSTR_IS_SHAPER(Y_AXIS)) { if (WITHIN(val, 0.01f, 1.0f)) { ftMotion.cfg.zeta[1] = val; - flag.update = true; + flag.update_shpr_params = true; } else SERIAL_ECHOLNPGM("Invalid Y zeta [", C('J'), "] value."); // Zeta Out of range @@ -409,7 +369,7 @@ void GcodeSuite::M493() { if (CMPNSTR_IS_EISHAPER(Y_AXIS)) { if (WITHIN(val, 0.00f, 1.0f)) { ftMotion.cfg.vtol[1] = val; - flag.update = true; + flag.update_shpr_params = true; } else SERIAL_ECHOLNPGM("Invalid Y vtol [", C('R'), "] value."); // VTol out of range. @@ -422,7 +382,7 @@ void GcodeSuite::M493() { planner.synchronize(); - if (flag.update) ftMotion.update_shaping_params(); + if (flag.update_shpr_params) ftMotion.update_shaping_params(); if (flag.reset_ft) { stepper.ftMotion_syncPosition(); diff --git a/Marlin/src/gcode/feature/ft_motion/M494.cpp b/Marlin/src/gcode/feature/ft_motion/M494.cpp new file mode 100644 index 0000000..a7672f3 --- /dev/null +++ b/Marlin/src/gcode/feature/ft_motion/M494.cpp @@ -0,0 +1,211 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2023 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(FT_MOTION) + +#include "../../gcode.h" +#include "../../../module/ft_motion.h" +#include "../../../module/stepper.h" + + +void say_ftm_cfg() { + #if ENABLED(FTM_UNIFIED_BWS) + SERIAL_ECHOPGM( "M494 FTMCFG FTM_BATCH_SIZE:", (int)(FTM_BW_SIZE), + " FTM_WINDOW_SIZE:", (int)(FTM_BW_SIZE)); + #else + SERIAL_ECHOPGM( "M494 FTMCFG FTM_BATCH_SIZE:", (int)(FTM_BATCH_SIZE), + " FTM_WINDOW_SIZE:", (int)(FTM_WINDOW_SIZE)); + #endif + SERIAL_ECHOPGM( " FTM_FS:", (int)(FTM_FS), + " FTM_STEPPER_FS:", (int)(FTM_STEPPER_FS), + " FTM_STEPPERCMD_BUFF_SIZE:", (int)(FTM_STEPPERCMD_BUFF_SIZE), + " STEPPER_TIMER_RATE:", (int)(STEPPER_TIMER_RATE), + " FTM_ZMAX:", (int)(FTM_ZMAX), + " X_MAX_LENGTH:", (int)(X_MAX_LENGTH)); + #if HAS_Y_AXIS + SERIAL_ECHOPGM( " Y_MAX_LENGTH:", (int)(Y_MAX_LENGTH)); + #endif + SERIAL_EOL(); +} + + +/** + * M494: Interact with Fixed-time Motion Control's trajectory generation. + * + * A Start / abort a frequency sweep profile. + * + * 0: None active. + * 1: Continuous sweep on X axis. + * 2: Continuous sweep on Y axis. + * 99: Abort the current sweep. + * + * B Start frequency. + * C End frequency. + * D Frequency rate. + * E Acceleration amplitude. + * F Step time. + * H Step acceleration amplitude. + * I Delay time to opening step. + * J Delay time from opening step to sweep. + * K Delay time from sweep to closing step. + * + * With no parameters passed, M494 will report the FTM configuration and + * variables required for autocalibration. + * + */ +void GcodeSuite::M494() { + + if (!parser.seen_any()) { say_ftm_cfg(); return; } + + if (!ftMotion.cfg.mode) { SERIAL_ECHOLN("M494 echo: rejected! FTM is not enabled."); return; } + + ftMotionTrajGenMode_t mode_val_seen = trajGenMode_NONE; + bool good_cfg_received = true; + + // Parse mode parameter. + if (parser.seenval('A')) { + const ftMotionTrajGenMode_t newmm = (ftMotionTrajGenMode_t)parser.value_byte(); + switch (newmm) { + default: SERIAL_ECHOLNPGM("?Invalid calibration mode [F] value."); return; + case trajGenMode_NONE: + case trajGenMode_SWEEPC_X: + #if HAS_Y_AXIS + case trajGenMode_SWEEPC_Y: + #endif + if ( ftMotion.traj_gen_cfg.mode ) { + SERIAL_ECHOLN("M494 echo: rejected! wait for the previous command to complete."); + return; + } + mode_val_seen = newmm; + break; + case trajGenMode_ABORT: + if ( ftMotion.traj_gen_cfg.mode ) { + ftMotion.traj_gen_cfg.mode = trajGenMode_NONE; + stepper.quick_stop(); + return; + } + } + } + + // Parse start frequency parameter. + if (parser.seenval('B')) { + const float val = parser.value_float(); + if (val >= 1.0f) ftMotion.traj_gen_cfg.f0 = val; + else { good_cfg_received = false; SERIAL_ECHOLN("M494 echo: Start frequency out of range."); } + } else good_cfg_received = false; + + // Parse end frequency parameter. + if (parser.seenval('C')) { + const float val = parser.value_float(); + if (val > 0.0f) ftMotion.traj_gen_cfg.f1 = val; + else { good_cfg_received = false; SERIAL_ECHOLN("M494 echo: End frequency out of range."); } + } else good_cfg_received = false; + + // Parse step frequency parameter. + if (parser.seenval('D')) { + const float val = parser.value_float(); + if (val > 0.0f) ftMotion.traj_gen_cfg.dfdt = val; + else { good_cfg_received = false; SERIAL_ECHOLN("M494 echo: Step / delta frequency out of range."); } + } else good_cfg_received = false; + + // Parse amplitude parameter. + if (parser.seenval('E')) { + const float val = parser.value_float(); + if (val > 0.0f) ftMotion.traj_gen_cfg.a = val; + else { good_cfg_received = false; SERIAL_ECHOLN("M494 echo: Amplitude out of range."); } + } else good_cfg_received = false; + + // Parse step time parameter. + if (parser.seenval('F')) { + const float val = parser.value_float(); + if (val > 0.0f) ftMotion.traj_gen_cfg.step_ti = val; + else { good_cfg_received = false; SERIAL_ECHOLN("M494 echo: Step time out of range."); } + } else good_cfg_received = false; + + // Parse step amplitude parameter. + if (parser.seenval('H')) { + const float val = parser.value_float(); + if (val > 0.0f) ftMotion.traj_gen_cfg.step_a = val; + else { good_cfg_received = false; SERIAL_ECHOLN("M494 echo: Step amplitude out of range."); } + } else good_cfg_received = false; + + // Parse delay parameter. + if (parser.seenval('I')) { + const float val = parser.value_float(); + if (val > 0.0f) ftMotion.traj_gen_cfg.dly1_ti = val; + else { good_cfg_received = false; SERIAL_ECHOLN("M494 echo: Delay 1 out of range."); } + } else good_cfg_received = false; + + // Parse delay parameter. + if (parser.seenval('J')) { + const float val = parser.value_float(); + if (val > 0.0f) ftMotion.traj_gen_cfg.dly2_ti = val; + else { good_cfg_received = false; SERIAL_ECHOLN("M494 echo: Delay 2 out of range."); } + } else good_cfg_received = false; + + // Parse delay parameter. + if (parser.seenval('K')) { + const float val = parser.value_float(); + if (val > 0.0f) ftMotion.traj_gen_cfg.dly3_ti = val; + else { good_cfg_received = false; SERIAL_ECHOLN("M494 echo: Delay 3 out of range."); } + } else good_cfg_received = false; + + // Validate and set. + if ( mode_val_seen && good_cfg_received ) { + + planner.synchronize(); + + const float f0_ = ftMotion.traj_gen_cfg.f0 - 1.f; // This is calculated assuming a ramp time of 1 sec. + + ftMotion.traj_gen_cfg.k1 = PI*ftMotion.traj_gen_cfg.dfdt; + ftMotion.traj_gen_cfg.k2 = 2.f*PI*f0_; + + ftMotion.traj_gen_cfg.pcws_ti[0] = ftMotion.traj_gen_cfg.dly1_ti; + ftMotion.traj_gen_cfg.pcws_ti[1] = ftMotion.traj_gen_cfg.pcws_ti[0] + 4 * ftMotion.traj_gen_cfg.step_ti; + ftMotion.traj_gen_cfg.pcws_ti[2] = ftMotion.traj_gen_cfg.pcws_ti[1] + ftMotion.traj_gen_cfg.dly2_ti; + ftMotion.traj_gen_cfg.pcws_ti[3] = ftMotion.traj_gen_cfg.pcws_ti[2] + ( ftMotion.traj_gen_cfg.f1 - f0_ ) / ftMotion.traj_gen_cfg.dfdt; + ftMotion.traj_gen_cfg.pcws_ti[4] = ftMotion.traj_gen_cfg.pcws_ti[3] + ftMotion.traj_gen_cfg.dly3_ti; + ftMotion.traj_gen_cfg.pcws_ti[5] = ftMotion.traj_gen_cfg.pcws_ti[4] + 4 * ftMotion.traj_gen_cfg.step_ti; + + ftMotion.traj_gen_cfg.step_a_x_0p5 = 0.5f * ftMotion.traj_gen_cfg.step_a; + ftMotion.traj_gen_cfg.step_a_x_step_ti_x_step_ti = ftMotion.traj_gen_cfg.step_a * sq(ftMotion.traj_gen_cfg.step_ti); + ftMotion.traj_gen_cfg.step_ti_x_2 = 2.f * ftMotion.traj_gen_cfg.step_ti; + ftMotion.traj_gen_cfg.step_ti_x_3 = 3.f * ftMotion.traj_gen_cfg.step_ti; + ftMotion.traj_gen_cfg.step_ti_x_4 = 4.f * ftMotion.traj_gen_cfg.step_ti; + + ftMotion.traj_gen_cfg.mode = mode_val_seen; + + ftMotion.setup_traj_gen(round(ftMotion.traj_gen_cfg.pcws_ti[5]*FTM_FS + 0.5f)); + + SERIAL_ECHOLN("M494 echo: SWEEPC starting."); + + stepper.enable_all_steppers(); + + // stepper.disable_all_steppers(); Leave this to the plugin or user; this can + // be annoying on printers that require all axes homing before axis movement. + } + +} + +#endif // FT_MOTION diff --git a/Marlin/src/gcode/gcode.cpp b/Marlin/src/gcode/gcode.cpp index 11b02de..0500efb 100644 --- a/Marlin/src/gcode/gcode.cpp +++ b/Marlin/src/gcode/gcode.cpp @@ -912,6 +912,7 @@ void GcodeSuite::process_parsed_command(const bool no_ok/*=false*/) { #if ENABLED(FT_MOTION) case 493: M493(); break; // M493: Fixed-Time Motion control + case 494: M494(); break; // M494: Fixed-Time Motion control trajectory generation #endif case 500: M500(); break; // M500: Store settings in EEPROM diff --git a/Marlin/src/gcode/gcode.h b/Marlin/src/gcode/gcode.h index 7ec3e00..e153064 100644 --- a/Marlin/src/gcode/gcode.h +++ b/Marlin/src/gcode/gcode.h @@ -606,7 +606,7 @@ class GcodeSuite { #if SAVED_POSITIONS static void G60(); - static void G61(int8_t slot=-1); + static void G61(); #endif #if ENABLED(GCODE_MOTION_MODES) @@ -1075,6 +1075,7 @@ class GcodeSuite { #if ENABLED(FT_MOTION) static void M493(); static void M493_report(const bool forReplay=true); + static void M494(); #endif static void M500(); diff --git a/Marlin/src/module/endstops.cpp b/Marlin/src/module/endstops.cpp index 03f5ab6..baaf3fe 100644 --- a/Marlin/src/module/endstops.cpp +++ b/Marlin/src/module/endstops.cpp @@ -50,6 +50,10 @@ #include "../feature/joystick.h" #endif +#if ENABLED(FT_MOTION) + #include "ft_motion.h" +#endif + #if HAS_BED_PROBE #include "probe.h" #endif @@ -834,8 +838,17 @@ void Endstops::update() { // Signal, after validation, if an endstop limit is pressed or not #if HAS_X_AXIS - if (stepper.axis_is_moving(X_AXIS)) { - if (!stepper.motor_direction(X_AXIS_HEAD)) { // -direction + #if ENABLED(FT_MOTION) + const bool x_moving_pos = ftMotion.axis_moving_pos(X_AXIS_HEAD), + x_moving_neg = ftMotion.axis_moving_neg(X_AXIS_HEAD); + #define X_MOVE_TEST x_moving_pos || x_moving_neg + #define X_NEG_DIR_TEST x_moving_neg + #else + #define X_MOVE_TEST stepper.axis_is_moving(X_AXIS) + #define X_NEG_DIR_TEST !stepper.motor_direction(X_AXIS_HEAD) + #endif + if (X_MOVE_TEST) { + if (X_NEG_DIR_TEST) { // -direction #if HAS_X_MIN_STATE PROCESS_ENDSTOP_X(MIN); #if CORE_DIAG(XY, Y, MIN) @@ -867,8 +880,17 @@ void Endstops::update() { #endif // HAS_X_AXIS #if HAS_Y_AXIS - if (stepper.axis_is_moving(Y_AXIS)) { - if (!stepper.motor_direction(Y_AXIS_HEAD)) { // -direction + #if ENABLED(FT_MOTION) + const bool y_moving_pos = ftMotion.axis_moving_pos(Y_AXIS_HEAD), + y_moving_neg = ftMotion.axis_moving_neg(Y_AXIS_HEAD); + #define Y_MOVE_TEST y_moving_pos || y_moving_neg + #define Y_NEG_DIR_TEST y_moving_neg + #else + #define Y_MOVE_TEST stepper.axis_is_moving(Y_AXIS) + #define Y_NEG_DIR_TEST !stepper.motor_direction(Y_AXIS_HEAD) + #endif + if (Y_MOVE_TEST) { + if (Y_NEG_DIR_TEST) { // -direction #if HAS_Y_MIN_STATE PROCESS_ENDSTOP_Y(MIN); #if CORE_DIAG(XY, X, MIN) @@ -900,8 +922,17 @@ void Endstops::update() { #endif // HAS_Y_AXIS #if HAS_Z_AXIS - if (stepper.axis_is_moving(Z_AXIS)) { - if (!stepper.motor_direction(Z_AXIS_HEAD)) { // Z -direction. Gantry down, bed up. + #if ENABLED(FT_MOTION) + const bool z_moving_pos = ftMotion.axis_moving_pos(Z_AXIS_HEAD), + z_moving_neg = ftMotion.axis_moving_neg(Z_AXIS_HEAD); + #define Z_MOVE_TEST z_moving_pos || z_moving_neg + #define Z_NEG_DIR_TEST z_moving_neg + #else + #define Z_MOVE_TEST stepper.axis_is_moving(Z_AXIS) + #define Z_NEG_DIR_TEST !stepper.motor_direction(Z_AXIS_HEAD) + #endif + if (Z_MOVE_TEST) { + if (Z_NEG_DIR_TEST) { // Z -direction. Gantry down, bed up. #if HAS_Z_MIN_STATE // If the Z_MIN_PIN is being used for the probe there's no // separate Z_MIN endstop. But a Z endstop could be wired @@ -945,6 +976,7 @@ void Endstops::update() { #endif // HAS_Z_AXIS #if HAS_I_AXIS + // TODO: FT_Motion logic. if (stepper.axis_is_moving(I_AXIS)) { if (!stepper.motor_direction(I_AXIS_HEAD)) { // -direction #if HAS_I_MIN_STATE diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index b19cd86..373fad4 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -30,6 +30,17 @@ FTMotion ftMotion; +#if !HAS_X_AXIS + static_assert(FTM_DEFAULT_X_COMPENSATOR == ftMotionCmpnstr_t_ZV, "ftMotionCmpnstr_t_ZV requires at least one linear axis."); + static_assert(FTM_DEFAULT_X_COMPENSATOR == ftMotionCmpnstr_t_ZVD, "ftMotionCmpnstr_t_ZVD requires at least one linear axis."); + static_assert(FTM_DEFAULT_X_COMPENSATOR == ftMotionCmpnstr_t_ZVDD, "ftMotionCmpnstr_t_ZVD requires at least one linear axis."); + static_assert(FTM_DEFAULT_X_COMPENSATOR == ftMotionCmpnstr_t_ZVDDD, "ftMotionCmpnstr_t_ZVD requires at least one linear axis."); + static_assert(FTM_DEFAULT_X_COMPENSATOR == ftMotionCmpnstr_t_EI, "ftMotionCmpnstr_t_EI requires at least one linear axis."); + static_assert(FTM_DEFAULT_X_COMPENSATOR == ftMotionCmpnstr_t_2HEI, "ftMotionCmpnstr_t_2HEI requires at least one linear axis."); + static_assert(FTM_DEFAULT_X_COMPENSATOR == ftMotionCmpnstr_t_3HEI, "ftMotionCmpnstr_t_3HEI requires at least one linear axis."); + static_assert(FTM_DEFAULT_X_COMPENSATOR == ftMotionCmpnstr_t_MZV, "ftMotionCmpnstr_t_MZV requires at least one linear axis."); +#endif + //----------------------------------------------------------------- // Variables. //----------------------------------------------------------------- @@ -37,12 +48,15 @@ FTMotion ftMotion; // Public variables. ft_config_t FTMotion::cfg; +ftMotionTrajGenConfig_t FTMotion::traj_gen_cfg; bool FTMotion::busy; // = false ft_command_t FTMotion::stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE] = {0U}; // Stepper commands buffer. int32_t FTMotion::stepperCmdBuff_produceIdx = 0, // Index of next stepper command write to the buffer. FTMotion::stepperCmdBuff_consumeIdx = 0; // Index of next stepper command read from the buffer. bool FTMotion::sts_stepperBusy = false; // The stepper buffer has items and is in use. +millis_t FTMotion::axis_pos_move_end_ti[NUM_AXIS_ENUMS] = {0}, + FTMotion::axis_neg_move_end_ti[NUM_AXIS_ENUMS] = {0}; // Private variables. @@ -91,9 +105,9 @@ uint32_t FTMotion::interpIdx = 0; // Index of current data point b #if HAS_X_AXIS FTMotion::shaping_t FTMotion::shaping = { 0, - x:{ false, { 0.0f }, { 0.0f }, { 0 }, { 0 } }, // ena, d_zi, Ai, Ni, max_i + x:{ false, { 0.0f }, { 0.0f }, { 0 }, 0 }, // ena, d_zi, Ai, Ni, max_i #if HAS_Y_AXIS - y:{ false, { 0.0f }, { 0.0f }, { 0 }, { 0 } } // ena, d_zi, Ai, Ni, max_i + y:{ false, { 0.0f }, { 0.0f }, { 0 }, 0 } // ena, d_zi, Ai, Ni, max_i #endif }; #endif @@ -117,86 +131,82 @@ static bool markBlockStart = false; // Controller main, to be invoked from non-isr task. void FTMotion::loop() { - if (!cfg.active) return; + if (!cfg.mode) return; - /** - * Handle block abort with the following sequence: - * 1. Zero out commands in stepper ISR. - * 2. Drain the motion buffer, stop processing until they are emptied. - * 3. Reset all the states / memory. - * 4. Signal ready for new block. - */ + // Handle block abort with the following sequence: + // 1. Zero out commands in stepper ISR. + // 2. Drain the motion buffer, stop processing until they are emptied. + // 3. Reset all the states / memory. + // 4. Signal ready for new block. if (stepper.abort_current_block) { if (sts_stepperBusy) return; // Wait until motion buffers are emptied discard_planner_block_protected(); reset(); + blockProcRdy = false; // Set queueing to look for next block. stepper.abort_current_block = false; // Abort finished. } - while (!blockProcRdy && (stepper.current_block = planner.get_current_block())) { - if (stepper.current_block->is_sync()) { // Sync block? - if (stepper.current_block->is_sync_pos()) // Position sync? Set the position. - stepper._set_position(stepper.current_block->position); + + while(!blockProcRdy && (stepper.current_block = planner.get_current_block())){ + if (stepper.current_block->is_sync()) { // Sync block? Sync the stepper counts and return. + TERN_(LASER_FEATURE, if (!(stepper.current_block->is_fan_sync() || stepper.current_block->is_pwr_sync()))) stepper._set_position(stepper.current_block->position); discard_planner_block_protected(); - continue; + } else { + loadBlockData(stepper.current_block); blockProcRdy = true; } - loadBlockData(stepper.current_block); - markBlockStart = true; - blockProcRdy = true; - // Some kinematics track axis motion in HX, HY, HZ - #if ANY(CORE_IS_XY, CORE_IS_XZ, MARKFORGED_XY, MARKFORGED_YX) - stepper.last_direction_bits.hx = stepper.current_block->direction_bits.hx; - #endif - #if ANY(CORE_IS_XY, CORE_IS_YZ, MARKFORGED_XY, MARKFORGED_YX) - stepper.last_direction_bits.hy = stepper.current_block->direction_bits.hy; - #endif - #if ANY(CORE_IS_XZ, CORE_IS_YZ) - stepper.last_direction_bits.hz = stepper.current_block->direction_bits.hz; - #endif } - if (blockProcRdy) { + if (blockProcRdy || traj_gen_cfg.mode) { + if (!batchRdy) makeVector(); // Caution: Do not consolidate checks on blockProcRdy/batchRdy, as they are written by makeVector(). // When makeVector is finished: either blockProcRdy has been set false (because the block is // done being processed) or batchRdy is set true, or both. // Check if the block has been completely converted: - if (!blockProcRdy) { + if (!blockProcRdy && !traj_gen_cfg.mode) { discard_planner_block_protected(); // Check if the block needs to be runout: if (!batchRdy && !planner.movesplanned()){ - runoutBlock(); - makeVector(); // Do an additional makeVector call to guarantee batchRdy set this loop. + runoutBlock(); makeVector(); // Do an additional makeVector call to guarantee batchRdy set this loop. } } } - // FBS / post processing. + + // Window/batch buffer management [and, optionally, FBS]. if (batchRdy && !batchRdyForInterp) { - // Call Ulendo FBS here. - #if ENABLED(FTM_UNIFIED_BWS) - trajMod = traj; // Move the window to traj + trajMod = traj; // Move the window to trajMod. #else - // Copy the uncompensated vectors. - #define TCOPY(A) memcpy(trajMod.A, traj.A, sizeof(trajMod.A)); - LOGICAL_AXIS_MAP_LC(TCOPY); - - // Shift the time series back in the window - #define TSHIFT(A) memcpy(traj.A, &traj.A[FTM_BATCH_SIZE], BATCH_SIDX_IN_WINDOW * sizeof(traj.A[0])); - LOGICAL_AXIS_MAP_LC(TSHIFT); + // Copy the trajectories not modified by FBS. + #define TCOPY(A) memcpy(trajMod.A, traj.A, sizeof(trajMod.A)) + LOGICAL_AXIS_CODE( + TCOPY(e), + TCOPY(x), TCOPY(y), TCOPY(z), + TCOPY(i), TCOPY(j), TCOPY(k), + TCOPY(u), TCOPY(v), TCOPY(w) + ); + + // Shift the time series back in the window. + #define TSHIFT(A) memcpy(traj.A, &traj.A[FTM_BATCH_SIZE], BATCH_SIDX_IN_WINDOW * sizeof(traj.A[0])) + LOGICAL_AXIS_CODE( + TSHIFT(e), + TSHIFT(x), TSHIFT(y), TSHIFT(z), + TSHIFT(i), TSHIFT(j), TSHIFT(k), + TSHIFT(u), TSHIFT(v), TSHIFT(w) + ); #endif - // ... data is ready in trajMod. - batchRdyForInterp = true; + batchRdyForInterp = true; // Indicate data is ready in trajMod. batchRdy = false; // Clear so makeVector() can resume generating points. } - // Interpolation. + + // Interpolation (generation of step commands from fixed time trajectory). while (batchRdyForInterp && (stepperCmdBuffItems() < (FTM_STEPPERCMD_BUFF_SIZE) - (FTM_STEPS_PER_UNIT_TIME))) { convertToSteps(interpIdx); @@ -207,53 +217,51 @@ void FTMotion::loop() { } // Report busy status to planner. - busy = (sts_stepperBusy || blockProcRdy || batchRdy || batchRdyForInterp); + busy = (sts_stepperBusy || blockProcRdy || batchRdy || batchRdyForInterp || traj_gen_cfg.mode); } #if HAS_X_AXIS // Refresh the gains used by shaping functions. - void FTMotion::AxisShaping::set_axis_shaping_A(const ftMotionShaper_t shaper, const_float_t zeta, const_float_t vtol) { + void FTMotion::AxisShaping::set_axis_shaping_A(const ftMotionCmpnstr_t shaper, const_float_t zeta, const_float_t vtol) { const float K = exp(-zeta * M_PI / sqrt(1.f - sq(zeta))), - K2 = sq(K), - K3 = K2 * K, - K4 = K3 * K; + K2 = sq(K); switch (shaper) { - case ftMotionShaper_ZV: + case ftMotionCmpnstr_ZV: max_i = 1U; Ai[0] = 1.0f / (1.0f + K); Ai[1] = Ai[0] * K; break; - case ftMotionShaper_ZVD: + case ftMotionCmpnstr_ZVD: max_i = 2U; Ai[0] = 1.0f / (1.0f + 2.0f * K + K2); Ai[1] = Ai[0] * 2.0f * K; Ai[2] = Ai[0] * K2; break; - case ftMotionShaper_ZVDD: + case ftMotionCmpnstr_ZVDD: max_i = 3U; - Ai[0] = 1.0f / (1.0f + 3.0f * K + 3.0f * K2 + K3); + Ai[0] = 1.0f / (1.0f + 3.0f * K + 3.0f * K2 + cu(K)); Ai[1] = Ai[0] * 3.0f * K; Ai[2] = Ai[0] * 3.0f * K2; - Ai[3] = Ai[0] * K3; + Ai[3] = Ai[0] * cu(K); break; - case ftMotionShaper_ZVDDD: + case ftMotionCmpnstr_ZVDDD: max_i = 4U; - Ai[0] = 1.0f / (1.0f + 4.0f * K + 6.0f * K2 + 4.0f * K3 + K4); + Ai[0] = 1.0f / (1.0f + 4.0f * K + 6.0f * K2 + 4.0f * cu(K) + sq(K2)); Ai[1] = Ai[0] * 4.0f * K; Ai[2] = Ai[0] * 6.0f * K2; - Ai[3] = Ai[0] * 4.0f * K3; - Ai[4] = Ai[0] * K4; + Ai[3] = Ai[0] * 4.0f * cu(K); + Ai[4] = Ai[0] * sq(K2); break; - case ftMotionShaper_EI: { + case ftMotionCmpnstr_EI: { max_i = 2U; Ai[0] = 0.25f * (1.0f + vtol); Ai[1] = 0.50f * (1.0f - vtol) * K; @@ -266,14 +274,14 @@ void FTMotion::loop() { } break; - case ftMotionShaper_2HEI: { + case ftMotionCmpnstr_2HEI: { max_i = 3U; const float vtolx2 = sq(vtol); const float X = pow(vtolx2 * (sqrt(1.0f - vtolx2) + 1.0f), 1.0f / 3.0f); Ai[0] = (3.0f * sq(X) + 2.0f * X + 3.0f * vtolx2) / (16.0f * X); Ai[1] = (0.5f - Ai[0]) * K; Ai[2] = Ai[1] * K; - Ai[3] = Ai[0] * K3; + Ai[3] = Ai[0] * cu(K); const float adj = 1.0f / (Ai[0] + Ai[1] + Ai[2] + Ai[3]); for (uint32_t i = 0U; i < 4U; i++) { @@ -282,13 +290,13 @@ void FTMotion::loop() { } break; - case ftMotionShaper_3HEI: { + case ftMotionCmpnstr_3HEI: { max_i = 4U; Ai[0] = 0.0625f * ( 1.0f + 3.0f * vtol + 2.0f * sqrt( 2.0f * ( vtol + 1.0f ) * vtol ) ); Ai[1] = 0.25f * ( 1.0f - vtol ) * K; Ai[2] = ( 0.5f * ( 1.0f + vtol ) - 2.0f * Ai[0] ) * K2; Ai[3] = Ai[1] * K2; - Ai[4] = Ai[0] * K4; + Ai[4] = Ai[0] * sq(K2); const float adj = 1.0f / (Ai[0] + Ai[1] + Ai[2] + Ai[3] + Ai[4]); for (uint32_t i = 0U; i < 5U; i++) { @@ -297,7 +305,7 @@ void FTMotion::loop() { } break; - case ftMotionShaper_MZV: { + case ftMotionCmpnstr_MZV: { max_i = 2U; const float Bx = 1.4142135623730950488016887242097f * K; Ai[0] = 1.0f / (1.0f + Bx + K2); @@ -314,32 +322,32 @@ void FTMotion::loop() { } // Refresh the indices used by shaping functions. - void FTMotion::AxisShaping::set_axis_shaping_N(const ftMotionShaper_t shaper, const_float_t f, const_float_t zeta) { + void FTMotion::AxisShaping::set_axis_shaping_N(const ftMotionCmpnstr_t shaper, const_float_t f, const_float_t zeta) { // Note that protections are omitted for DBZ and for index exceeding array length. const float df = sqrt ( 1.f - sq(zeta) ); switch (shaper) { - case ftMotionShaper_ZV: + case ftMotionCmpnstr_ZV: Ni[1] = round((0.5f / f / df) * (FTM_FS)); break; - case ftMotionShaper_ZVD: - case ftMotionShaper_EI: + case ftMotionCmpnstr_ZVD: + case ftMotionCmpnstr_EI: Ni[1] = round((0.5f / f / df) * (FTM_FS)); Ni[2] = Ni[1] + Ni[1]; break; - case ftMotionShaper_ZVDD: - case ftMotionShaper_2HEI: + case ftMotionCmpnstr_ZVDD: + case ftMotionCmpnstr_2HEI: Ni[1] = round((0.5f / f / df) * (FTM_FS)); Ni[2] = Ni[1] + Ni[1]; Ni[3] = Ni[2] + Ni[1]; break; - case ftMotionShaper_ZVDDD: - case ftMotionShaper_3HEI: + case ftMotionCmpnstr_ZVDDD: + case ftMotionCmpnstr_3HEI: Ni[1] = round((0.5f / f / df) * (FTM_FS)); Ni[2] = Ni[1] + Ni[1]; Ni[3] = Ni[2] + Ni[1]; Ni[4] = Ni[3] + Ni[1]; break; - case ftMotionShaper_MZV: + case ftMotionCmpnstr_MZV: Ni[1] = round((0.375f / f / df) * (FTM_FS)); Ni[2] = Ni[1] + Ni[1]; break; @@ -348,19 +356,13 @@ void FTMotion::loop() { } void FTMotion::update_shaping_params() { - #if HAS_X_AXIS - shaping.x.ena = CMPNSTR_HAS_SHAPER(X_AXIS); - if (shaping.x.ena) { - shaping.x.set_axis_shaping_A(cfg.shaper[X_AXIS], cfg.zeta[X_AXIS], cfg.vtol[X_AXIS]); - shaping.x.set_axis_shaping_N(cfg.shaper[X_AXIS], cfg.baseFreq[X_AXIS], cfg.zeta[X_AXIS]); - } - #endif + shaping.x.set_axis_shaping_A(cfg.cmpnstr[X_AXIS], cfg.zeta[X_AXIS], cfg.vtol[X_AXIS]); + shaping.x.set_axis_shaping_N(cfg.cmpnstr[X_AXIS], cfg.baseFreq[X_AXIS], cfg.zeta[X_AXIS]); + shaping.x.ena = WITHIN(cfg.cmpnstr[X_AXIS], ftMotionCmpnstr_ZV, ftMotionCmpnstr_MZV); #if HAS_Y_AXIS - shaping.y.ena = CMPNSTR_HAS_SHAPER(Y_AXIS); - if (shaping.y.ena) { - shaping.y.set_axis_shaping_A(cfg.shaper[Y_AXIS], cfg.zeta[Y_AXIS], cfg.vtol[Y_AXIS]); - shaping.y.set_axis_shaping_N(cfg.shaper[Y_AXIS], cfg.baseFreq[Y_AXIS], cfg.zeta[Y_AXIS]); - } + shaping.y.set_axis_shaping_N(cfg.cmpnstr[Y_AXIS], cfg.baseFreq[Y_AXIS], cfg.zeta[Y_AXIS]); + shaping.y.set_axis_shaping_A(cfg.cmpnstr[Y_AXIS], cfg.zeta[Y_AXIS], cfg.vtol[Y_AXIS]); + shaping.y.ena = WITHIN(cfg.cmpnstr[Y_AXIS], ftMotionCmpnstr_ZV, ftMotionCmpnstr_MZV); #endif } @@ -378,7 +380,7 @@ void FTMotion::reset() { endPosn_prevBlock.reset(); makeVector_idx = 0; - makeVector_batchIdx = TERN(FTM_UNIFIED_BWS, 0, _MIN(BATCH_SIDX_IN_WINDOW, FTM_BATCH_SIZE)); + makeVector_batchIdx = BATCH_SIDX_IN_WINDOW; steps.reset(); interpIdx = 0; @@ -392,8 +394,51 @@ void FTMotion::reset() { #endif TERN_(HAS_EXTRUDERS, e_raw_z1 = e_advanced_z1 = 0.0f); + + NUM_AXIS_CODE( + axis_pos_move_end_ti[A_AXIS] = 0, + axis_pos_move_end_ti[B_AXIS] = 0, + axis_pos_move_end_ti[C_AXIS] = 0, + axis_pos_move_end_ti[I_AXIS] = 0, + axis_pos_move_end_ti[J_AXIS] = 0, + axis_pos_move_end_ti[K_AXIS] = 0, + axis_pos_move_end_ti[U_AXIS] = 0, + axis_pos_move_end_ti[V_AXIS] = 0, + axis_pos_move_end_ti[W_AXIS] = 0 + ); + #if ANY(CORE_IS_XY, MARKFORGED_XY, MARKFORGED_YX) + axis_pos_move_end_ti[X_HEAD] = 0; + axis_pos_move_end_ti[Y_HEAD] = 0; + #endif + + NUM_AXIS_CODE( + axis_neg_move_end_ti[A_AXIS] = 0, + axis_neg_move_end_ti[B_AXIS] = 0, + axis_neg_move_end_ti[C_AXIS] = 0, + axis_neg_move_end_ti[I_AXIS] = 0, + axis_neg_move_end_ti[J_AXIS] = 0, + axis_neg_move_end_ti[K_AXIS] = 0, + axis_neg_move_end_ti[U_AXIS] = 0, + axis_neg_move_end_ti[V_AXIS] = 0, + axis_neg_move_end_ti[W_AXIS] = 0 + ); + #if ANY(CORE_IS_XY, MARKFORGED_XY, MARKFORGED_YX) + axis_neg_move_end_ti[X_HEAD] = 0; + axis_neg_move_end_ti[Y_HEAD] = 0; + #endif + } + +void FTMotion::setup_traj_gen(uint32_t intervals) { + // Extend the intervals by propagation time and some margin to + // ensure motion is complete when completion message is sent. + max_intervals = FTM_BATCH_SIZE*(PROP_BATCHES+ceil(((float)(intervals)+FTM_FS*(0.5f+(float)(FTM_STEPPERCMD_BUFF_SIZE)/float(FTM_STEPPER_FS)))/FTM_BATCH_SIZE)); + reset(); + busy = true; +} + + // Private functions. void FTMotion::discard_planner_block_protected() { @@ -420,16 +465,20 @@ void FTMotion::runoutBlock() { ratio.reset(); int32_t n_to_fill_batch = FTM_WINDOW_SIZE - makeVector_batchIdx; - - // This line is to be modified for FBS use; do not optimize out. - int32_t n_to_settle_cmpnstr = (TERN_(HAS_X_AXIS, shaping.x.ena) || TERN_(HAS_Y_AXIS, shaping.y.ena )) ? FTM_ZMAX : 0; + + // This line or function is to be modified for FBS use; do not optimize out. + int32_t n_to_settle_cmpnstr = num_samples_cmpnstr_settle(); int32_t n_to_fill_batch_after_settling = (n_to_settle_cmpnstr > n_to_fill_batch) ? FTM_BATCH_SIZE - ((n_to_settle_cmpnstr - n_to_fill_batch) % FTM_BATCH_SIZE) : n_to_fill_batch - n_to_settle_cmpnstr; int32_t n_to_settle_and_fill_batch = n_to_settle_cmpnstr + n_to_fill_batch_after_settling; - max_intervals = PROP_BATCHES * FTM_BATCH_SIZE + n_to_settle_and_fill_batch; + int32_t N_needed_to_propagate_to_stepper = PROP_BATCHES; + + int32_t n_to_use = N_needed_to_propagate_to_stepper*FTM_BATCH_SIZE + n_to_settle_and_fill_batch; + + max_intervals = n_to_use; blockProcRdy = true; } @@ -446,14 +495,14 @@ void FTMotion::init() { reset(); // Precautionary. } -// Load / convert block data from planner to fixed-time control variables. +// Loads / converts block data from planner to fixed-time control variables. void FTMotion::loadBlockData(block_t * const current_block) { const float totalLength = current_block->millimeters, oneOverLength = 1.0f / totalLength; startPosn = endPosn_prevBlock; - const xyze_pos_t moveDist = LOGICAL_AXIS_ARRAY( + xyze_pos_t moveDist = LOGICAL_AXIS_ARRAY( current_block->steps.e * planner.mm_per_step[E_AXIS_N(current_block->extruder)] * (current_block->direction_bits.e ? 1 : -1), current_block->steps.x * planner.mm_per_step[X_AXIS] * (current_block->direction_bits.x ? 1 : -1), current_block->steps.y * planner.mm_per_step[Y_AXIS] * (current_block->direction_bits.y ? 1 : -1), @@ -530,8 +579,7 @@ void FTMotion::loadBlockData(block_t * const current_block) { * f_s * T1_P : (mm) Distance traveled during the accel phase * f_e * T3_P : (mm) Distance traveled during the decel phase */ - const float adist = f_s * T1_P; - F_P = (2.0f * totalLength - adist - f_e * T3_P) / (T1_P + 2.0f * T2_P + T3_P); // (mm/s) Feedrate at the end of the accel phase + F_P = (2.0f * totalLength - f_s * T1_P - f_e * T3_P) / (T1_P + 2.0f * T2_P + T3_P); // (mm/s) Feedrate at the end of the accel phase // Calculate the acceleration and deceleration rates accel_P = N1 ? ((F_P - f_s) / T1_P) : 0.0f; @@ -539,7 +587,7 @@ void FTMotion::loadBlockData(block_t * const current_block) { decel_P = (f_e - F_P) / T3_P; // Calculate the distance traveled during the accel phase - s_1e = adist + 0.5f * accel_P * sq(T1_P); + s_1e = f_s * T1_P + 0.5f * accel_P * sq(T1_P); // Calculate the distance traveled during the decel phase s_2e = s_1e + F_P * T2_P; @@ -549,81 +597,132 @@ void FTMotion::loadBlockData(block_t * const current_block) { endPosn_prevBlock += moveDist; + millis_t move_end_ti = millis() + SEC_TO_MS(FTM_TS*(float)(max_intervals + num_samples_cmpnstr_settle() + (PROP_BATCHES+1)*FTM_BATCH_SIZE) + ((float)FTM_STEPPERCMD_BUFF_SIZE/(float)FTM_STEPPER_FS)); + + #if CORE_IS_XY + if (moveDist.x > 0.f) axis_pos_move_end_ti[A_AXIS] = move_end_ti; + if (moveDist.y > 0.f) axis_pos_move_end_ti[B_AXIS] = move_end_ti; + if (moveDist.x + moveDist.y > 0.f) axis_pos_move_end_ti[X_HEAD] = move_end_ti; + if (moveDist.x - moveDist.y > 0.f) axis_pos_move_end_ti[Y_HEAD] = move_end_ti; + if (moveDist.x < 0.f) axis_neg_move_end_ti[A_AXIS] = move_end_ti; + if (moveDist.y < 0.f) axis_neg_move_end_ti[B_AXIS] = move_end_ti; + if (moveDist.x + moveDist.y < 0.f) axis_neg_move_end_ti[X_HEAD] = move_end_ti; + if (moveDist.x - moveDist.y < 0.f) axis_neg_move_end_ti[Y_HEAD] = move_end_ti; + #else + if (moveDist.x > 0.f) axis_pos_move_end_ti[X_AXIS] = move_end_ti; + if (moveDist.y > 0.f) axis_pos_move_end_ti[Y_AXIS] = move_end_ti; + if (moveDist.x < 0.f) axis_neg_move_end_ti[X_AXIS] = move_end_ti; + if (moveDist.y < 0.f) axis_neg_move_end_ti[Y_AXIS] = move_end_ti; + #endif + if (moveDist.z > 0.f) axis_pos_move_end_ti[Z_AXIS] = move_end_ti; + if (moveDist.z < 0.f) axis_neg_move_end_ti[Z_AXIS] = move_end_ti; + // if (moveDist.i > 0.f) axis_pos_move_end_ti[I_AXIS] = move_end_ti; + // if (moveDist.i < 0.f) axis_neg_move_end_ti[I_AXIS] = move_end_ti; + // if (moveDist.j > 0.f) axis_pos_move_end_ti[J_AXIS] = move_end_ti; + // if (moveDist.j < 0.f) axis_neg_move_end_ti[J_AXIS] = move_end_ti; + // if (moveDist.k > 0.f) axis_pos_move_end_ti[K_AXIS] = move_end_ti; + // if (moveDist.k < 0.f) axis_neg_move_end_ti[K_AXIS] = move_end_ti; + // if (moveDist.u > 0.f) axis_pos_move_end_ti[U_AXIS] = move_end_ti; + // if (moveDist.u < 0.f) axis_neg_move_end_ti[U_AXIS] = move_end_ti; + // . + // . + // . + + // If the endstop is already pressed, endstop interrupts won't invoke + // endstop_triggered and the move will grind. So check here for a + // triggered endstop, which shortly marks the block for discard. + endstops.update(); + } // Generate data points of the trajectory. void FTMotion::makeVector() { - float accel_k = 0.0f; // (mm/s^2) Acceleration K factor - float tau = (makeVector_idx + 1) * (FTM_TS); // (s) Time since start of block - float dist = 0.0f; // (mm) Distance traveled - - if (makeVector_idx < N1) { - // Acceleration phase - dist = (f_s * tau) + (0.5f * accel_P * sq(tau)); // (mm) Distance traveled for acceleration phase since start of block - accel_k = accel_P; // (mm/s^2) Acceleration K factor from Accel phase - } - else if (makeVector_idx < (N1 + N2)) { - // Coasting phase - dist = s_1e + F_P * (tau - N1 * (FTM_TS)); // (mm) Distance traveled for coasting phase since start of block - } - else { - // Deceleration phase - tau -= (N1 + N2) * (FTM_TS); // (s) Time since start of decel phase - dist = s_2e + F_P * tau + 0.5f * decel_P * sq(tau); // (mm) Distance traveled for deceleration phase since start of block - accel_k = decel_P; // (mm/s^2) Acceleration K factor from Decel phase - } - - #define _FTM_TRAJ(A) traj.A[makeVector_batchIdx] = startPosn.A + ratio.A * dist; - LOGICAL_AXIS_MAP_LC(_FTM_TRAJ); + do { + if (traj_gen_cfg.mode == trajGenMode_SWEEPC_X || traj_gen_cfg.mode == trajGenMode_SWEEPC_Y) { + float s_ = 0.0f; + + const float tau = makeVector_idx * FTM_TS; + + if ( tau <= traj_gen_cfg.pcws_ti[0] ){ s_ = 0.f; } + + else if ( tau <= traj_gen_cfg.pcws_ti[1] ){ + const float tau_ = tau - traj_gen_cfg.pcws_ti[0]; + if (tau_ < traj_gen_cfg.step_ti){ s_ = traj_gen_cfg.step_a_x_0p5 * sq(tau_); } + else if (tau_ < traj_gen_cfg.step_ti_x_3){ const float k_ = tau_ - traj_gen_cfg.step_ti_x_2; s_ = traj_gen_cfg.step_a_x_step_ti_x_step_ti - traj_gen_cfg.step_a_x_0p5 * sq(k_); } + else { const float k_ = tau_ - traj_gen_cfg.step_ti_x_4; s_ = traj_gen_cfg.step_a_x_0p5 * sq(k_); } + } + else if ( tau <= traj_gen_cfg.pcws_ti[2] ){ s_ = 0.0f; } + + else if ( tau <= traj_gen_cfg.pcws_ti[3] ){ + const float tau_ = tau - traj_gen_cfg.pcws_ti[2]; + const float tau_tau_ = sq(tau_); + const float tau_tau_tau_ = tau_ * tau_tau_; + const float k_ = 1.f / (2.f*traj_gen_cfg.k1*tau_ + traj_gen_cfg.k2); + const float A_ = ((tau_tau_tau_ < 1.f) ? tau_tau_tau_ : 1.f) * traj_gen_cfg.a * sq(k_); + s_ = A_ * sin( traj_gen_cfg.k1*tau_tau_ + traj_gen_cfg.k2*tau_ ); + } + else if ( tau <= traj_gen_cfg.pcws_ti[4] ){ s_ = 0.f; } - #if HAS_EXTRUDERS - if (cfg.linearAdvEna) { - float dedt_adj = (traj.e[makeVector_batchIdx] - e_raw_z1) * (FTM_FS); - if (ratio.e > 0.0f) dedt_adj += accel_k * cfg.linearAdvK * 0.0001f; + else if ( tau <= traj_gen_cfg.pcws_ti[5] ){ + const float tau_ = tau - traj_gen_cfg.pcws_ti[4]; + if (tau_ < traj_gen_cfg.step_ti){ s_ = -traj_gen_cfg.step_a_x_0p5 * sq(tau_); } + else if (tau_ < traj_gen_cfg.step_ti_x_3){ const float k_ = tau_ - traj_gen_cfg.step_ti_x_2; s_ = -traj_gen_cfg.step_a_x_step_ti_x_step_ti + traj_gen_cfg.step_a_x_0p5 * sq(k_); } + else { const float k_ = tau_ - traj_gen_cfg.step_ti_x_4; s_ = -traj_gen_cfg.step_a_x_0p5 * sq(k_); } + } + else { s_ = 0.f; } - e_raw_z1 = traj.e[makeVector_batchIdx]; - e_advanced_z1 += dedt_adj * (FTM_TS); - traj.e[makeVector_batchIdx] = e_advanced_z1; - } - #endif + if (traj_gen_cfg.mode == trajGenMode_SWEEPC_X ) traj.x[makeVector_batchIdx] = s_; + else traj.y[makeVector_batchIdx] = s_; - // Update shaping parameters if needed. - - switch (cfg.dynFreqMode) { - - #if HAS_DYNAMIC_FREQ_MM - case dynFreqMode_Z_BASED: - if (traj.z[makeVector_batchIdx] != 0.0f) { // Only update if Z changed. - #if HAS_X_AXIS - const float xf = cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * traj.z[makeVector_batchIdx]; - shaping.x.set_axis_shaping_N(cfg.shaper[X_AXIS], _MAX(xf, FTM_MIN_SHAPE_FREQ), cfg.zeta[X_AXIS]); - #endif - #if HAS_Y_AXIS - const float yf = cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * traj.z[makeVector_batchIdx]; - shaping.y.set_axis_shaping_N(cfg.shaper[Y_AXIS], _MAX(yf, FTM_MIN_SHAPE_FREQ), cfg.zeta[Y_AXIS]); - #endif - } - break; - #endif + } else { // Planner mode. + float accel_k = 0.0f; // (mm/s^2) Acceleration K factor + float tau = (makeVector_idx + 1) * (FTM_TS); // (s) Time since start of block + float dist = 0.0f; // (mm) Distance traveled - #if HAS_DYNAMIC_FREQ_G - case dynFreqMode_MASS_BASED: - // Update constantly. The optimization done for Z value makes - // less sense for E, as E is expected to constantly change. - #if HAS_X_AXIS - shaping.x.set_axis_shaping_N(cfg.shaper[X_AXIS], cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * traj.e[makeVector_batchIdx], cfg.zeta[X_AXIS]); - #endif - #if HAS_Y_AXIS - shaping.y.set_axis_shaping_N(cfg.shaper[Y_AXIS], cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * traj.e[makeVector_batchIdx], cfg.zeta[Y_AXIS]); - #endif - break; - #endif + if (makeVector_idx < N1) { + // Acceleration phase + dist = (f_s * tau) + (0.5f * accel_P * sq(tau)); // (mm) Distance traveled for acceleration phase since start of block + accel_k = accel_P; // (mm/s^2) Acceleration K factor from Accel phase + } + else if (makeVector_idx < (N1 + N2)) { + // Coasting phase + dist = s_1e + F_P * (tau - N1 * (FTM_TS)); // (mm) Distance traveled for coasting phase since start of block + //accel_k = 0.0f; + } + else { + // Deceleration phase + tau -= (N1 + N2) * (FTM_TS); // (s) Time since start of decel phase + dist = s_2e + F_P * tau + 0.5f * decel_P * sq(tau); // (mm) Distance traveled for deceleration phase since start of block + accel_k = decel_P; // (mm/s^2) Acceleration K factor from Decel phase + } - default: break; - } + LOGICAL_AXIS_CODE( + traj.e[makeVector_batchIdx] = startPosn.e + ratio.e * dist, + traj.x[makeVector_batchIdx] = startPosn.x + ratio.x * dist, + traj.y[makeVector_batchIdx] = startPosn.y + ratio.y * dist, + traj.z[makeVector_batchIdx] = startPosn.z + ratio.z * dist, + traj.i[makeVector_batchIdx] = startPosn.i + ratio.i * dist, + traj.j[makeVector_batchIdx] = startPosn.j + ratio.j * dist, + traj.k[makeVector_batchIdx] = startPosn.k + ratio.k * dist, + traj.u[makeVector_batchIdx] = startPosn.u + ratio.u * dist, + traj.v[makeVector_batchIdx] = startPosn.v + ratio.v * dist, + traj.w[makeVector_batchIdx] = startPosn.w + ratio.w * dist + ); + + #if HAS_EXTRUDERS + if (cfg.linearAdvEna) { + float dedt_adj = (traj.e[makeVector_batchIdx] - e_raw_z1) * (FTM_FS); + if (ratio.e > 0.0f) dedt_adj += accel_k * cfg.linearAdvK; + + e_raw_z1 = traj.e[makeVector_batchIdx]; + e_advanced_z1 += dedt_adj * (FTM_TS); + traj.e[makeVector_batchIdx] = e_advanced_z1; + } + #endif + } - // Apply shaping if active on each axis - #if HAS_X_AXIS + // Apply shaping. + #if HAS_X_AXIS if (shaping.x.ena) { shaping.x.d_zi[shaping.zi_idx] = traj.x[makeVector_batchIdx]; traj.x[makeVector_batchIdx] *= shaping.x.Ai[0]; @@ -646,16 +745,18 @@ void FTMotion::makeVector() { if (++shaping.zi_idx == (FTM_ZMAX)) shaping.zi_idx = 0; #endif - // Filled up the queue with regular and shaped steps - if (++makeVector_batchIdx == FTM_WINDOW_SIZE) { - makeVector_batchIdx = BATCH_SIDX_IN_WINDOW; - batchRdy = true; - } + // Filled up the queue with regular and shaped steps + if (++makeVector_batchIdx == FTM_WINDOW_SIZE) { + makeVector_batchIdx = BATCH_SIDX_IN_WINDOW; + batchRdy = true; + } - if (++makeVector_idx == max_intervals) { - blockProcRdy = false; - makeVector_idx = 0; - } + if (++makeVector_idx == max_intervals) { + if (traj_gen_cfg.mode) SERIAL_ECHOLN("M494 echo: profile ran to completion."); + blockProcRdy = false; traj_gen_cfg.mode = trajGenMode_NONE; + makeVector_idx = 0; + } + } while ((blockProcRdy || traj_gen_cfg.mode) && !batchRdy); } /** @@ -664,7 +765,7 @@ void FTMotion::makeVector() { * - Tests for delta are moved outside the loop. * - Two functions are used for command computation with an array of function pointers. */ -static void (*command_set[SUM_TERN(HAS_EXTRUDERS, NUM_AXES, 1)])(int32_t&, int32_t&, ft_command_t&, int32_t, int32_t); +static void (*command_set[NUM_AXES TERN0(HAS_EXTRUDERS, +1)])(int32_t&, int32_t&, ft_command_t&, int32_t, int32_t); static void command_set_pos(int32_t &e, int32_t &s, ft_command_t &b, int32_t bd, int32_t bs) { if (e < FTM_CTS_COMPARE_VAL) return; @@ -688,7 +789,7 @@ void FTMotion::convertToSteps(const uint32_t idx) { #if ENABLED(STEPS_ROUNDING) #define TOSTEPS(A,B) int32_t(trajMod.A[idx] * planner.settings.axis_steps_per_mm[B] + (trajMod.A[idx] < 0.0f ? -0.5f : 0.5f)) const xyze_long_t steps_tar = LOGICAL_AXIS_ARRAY( - TOSTEPS(e, E_AXIS_N(stepper.current_block->extruder)), // May be eliminated if guaranteed positive. + TOSTEPS(e, E_AXIS_N(current_block->extruder)), // May be eliminated if guaranteed positive. TOSTEPS(x, X_AXIS), TOSTEPS(y, Y_AXIS), TOSTEPS(z, Z_AXIS), TOSTEPS(i, I_AXIS), TOSTEPS(j, J_AXIS), TOSTEPS(k, K_AXIS), TOSTEPS(u, U_AXIS), TOSTEPS(v, V_AXIS), TOSTEPS(w, W_AXIS) @@ -697,22 +798,32 @@ void FTMotion::convertToSteps(const uint32_t idx) { #else #define TOSTEPS(A,B) int32_t(trajMod.A[idx] * planner.settings.axis_steps_per_mm[B]) - steps.A xyze_long_t delta = LOGICAL_AXIS_ARRAY( - TOSTEPS(e, E_AXIS_N(stepper.current_block->extruder)), + TOSTEPS(e, E_AXIS_N(current_block->extruder)), TOSTEPS(x, X_AXIS), TOSTEPS(y, Y_AXIS), TOSTEPS(z, Z_AXIS), TOSTEPS(i, I_AXIS), TOSTEPS(j, J_AXIS), TOSTEPS(k, K_AXIS), TOSTEPS(u, U_AXIS), TOSTEPS(v, V_AXIS), TOSTEPS(w, W_AXIS) ); #endif - #define _COMMAND_SET(AXIS) command_set[_AXIS(AXIS)] = delta[_AXIS(AXIS)] >= 0 ? command_set_pos : command_set_neg; - LOGICAL_AXIS_MAP(_COMMAND_SET); + LOGICAL_AXIS_CODE( + command_set[E_AXIS_N(current_block->extruder)] = delta.e >= 0 ? command_set_pos : command_set_neg, + command_set[X_AXIS] = delta.x >= 0 ? command_set_pos : command_set_neg, + command_set[Y_AXIS] = delta.y >= 0 ? command_set_pos : command_set_neg, + command_set[Z_AXIS] = delta.z >= 0 ? command_set_pos : command_set_neg, + command_set[I_AXIS] = delta.i >= 0 ? command_set_pos : command_set_neg, + command_set[J_AXIS] = delta.j >= 0 ? command_set_pos : command_set_neg, + command_set[K_AXIS] = delta.k >= 0 ? command_set_pos : command_set_neg, + command_set[U_AXIS] = delta.u >= 0 ? command_set_pos : command_set_neg, + command_set[V_AXIS] = delta.v >= 0 ? command_set_pos : command_set_neg, + command_set[W_AXIS] = delta.w >= 0 ? command_set_pos : command_set_neg + ); for (uint32_t i = 0U; i < (FTM_STEPS_PER_UNIT_TIME); i++) { ft_command_t &cmd = stepperCmdBuff[stepperCmdBuff_produceIdx]; // Init all step/dir bits to 0 (defaulting to reverse/negative motion) - cmd = 0; + stepperCmdBuff[stepperCmdBuff_produceIdx] = 0; // Mark the start of a new block if (markBlockStart) { @@ -724,8 +835,18 @@ void FTMotion::convertToSteps(const uint32_t idx) { err_P += delta; // Set up step/dir bits for all axes - #define _COMMAND_RUN(AXIS) command_set[_AXIS(AXIS)](err_P[_AXIS(AXIS)], steps[_AXIS(AXIS)], cmd, _BV(FT_BIT_DIR_##AXIS), _BV(FT_BIT_STEP_##AXIS)); - LOGICAL_AXIS_MAP(_COMMAND_RUN); + LOGICAL_AXIS_CODE( + command_set[E_AXIS_N(current_block->extruder)](err_P.e, steps.e, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_E), _BV(FT_BIT_STEP_E)), + command_set[X_AXIS](err_P.x, steps.x, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_X), _BV(FT_BIT_STEP_X)), + command_set[Y_AXIS](err_P.y, steps.y, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_Y), _BV(FT_BIT_STEP_Y)), + command_set[Z_AXIS](err_P.z, steps.z, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_Z), _BV(FT_BIT_STEP_Z)), + command_set[I_AXIS](err_P.i, steps.i, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_I), _BV(FT_BIT_STEP_I)), + command_set[J_AXIS](err_P.j, steps.j, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_J), _BV(FT_BIT_STEP_J)), + command_set[K_AXIS](err_P.k, steps.k, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_K), _BV(FT_BIT_STEP_K)), + command_set[U_AXIS](err_P.u, steps.u, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_U), _BV(FT_BIT_STEP_U)), + command_set[V_AXIS](err_P.v, steps.v, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_V), _BV(FT_BIT_STEP_V)), + command_set[W_AXIS](err_P.w, steps.w, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_W), _BV(FT_BIT_STEP_W)), + ); // Next circular buffer index if (++stepperCmdBuff_produceIdx == (FTM_STEPPERCMD_BUFF_SIZE)) diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h index 4851a8c..2cad21a 100644 --- a/Marlin/src/module/ft_motion.h +++ b/Marlin/src/module/ft_motion.h @@ -38,23 +38,17 @@ typedef struct FTConfig { bool active = ENABLED(FTM_IS_DEFAULT_MOTION); // Active (else standard motion) + ftMotionMode_t mode = FTM_DEFAULT_MODE; // Mode / active compensation mode configuration. #if HAS_X_AXIS - ftMotionShaper_t shaper[1 + ENABLED(HAS_Y_AXIS)] = // Shaper type - { FTM_DEFAULT_SHAPER_X OPTARG(HAS_Y_AXIS, FTM_DEFAULT_SHAPER_Y) }; + ftMotionCmpnstr_t cmpnstr[1 + ENABLED(HAS_Y_AXIS)] = // Compensation mode. + { FTM_DEFAULT_X_COMPENSATOR OPTARG(HAS_Y_AXIS, FTM_DEFAULT_Y_COMPENSATOR) }; float baseFreq[1 + ENABLED(HAS_Y_AXIS)] = // Base frequency. [Hz] { FTM_SHAPING_DEFAULT_X_FREQ OPTARG(HAS_Y_AXIS, FTM_SHAPING_DEFAULT_Y_FREQ) }; float zeta[1 + ENABLED(HAS_Y_AXIS)] = // Damping factor - { FTM_SHAPING_ZETA_X OPTARG(HAS_Y_AXIS, FTM_SHAPING_ZETA_Y) }; + { FTM_SHAPING_DEFAULT_ZETA_X OPTARG(HAS_Y_AXIS, FTM_SHAPING_DEFAULT_ZETA_Y) }; float vtol[1 + ENABLED(HAS_Y_AXIS)] = // Vibration Level - { FTM_SHAPING_V_TOL_X OPTARG(HAS_Y_AXIS, FTM_SHAPING_V_TOL_Y) }; - #endif - - #if HAS_DYNAMIC_FREQ - dynFreqMode_t dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE; // Dynamic frequency mode configuration. - float dynFreqK[1 + ENABLED(HAS_Y_AXIS)] = { 0.0f }; // Scaling / gain for dynamic frequency. [Hz/mm] or [Hz/g] - #else - static constexpr dynFreqMode_t dynFreqMode = dynFreqMode_DISABLED; + { FTM_SHAPING_DEFAULT_V_TOL_X OPTARG(HAS_Y_AXIS, FTM_SHAPING_DEFAULT_V_TOL_Y) }; #endif #if HAS_EXTRUDERS @@ -69,36 +63,32 @@ class FTMotion { // Public variables static ft_config_t cfg; + static ftMotionTrajGenConfig_t traj_gen_cfg; static bool busy; static void set_defaults() { - cfg.active = ENABLED(FTM_IS_DEFAULT_MOTION); + cfg.mode = FTM_DEFAULT_MODE; - #if HAS_X_AXIS - cfg.shaper[X_AXIS] = FTM_DEFAULT_SHAPER_X; - cfg.baseFreq[X_AXIS] = FTM_SHAPING_DEFAULT_X_FREQ; - cfg.zeta[X_AXIS] = FTM_SHAPING_ZETA_X; - cfg.vtol[X_AXIS] = FTM_SHAPING_V_TOL_X; - #endif + TERN_(HAS_X_AXIS, cfg.cmpnstr[X_AXIS] = FTM_DEFAULT_X_COMPENSATOR); + TERN_(HAS_Y_AXIS, cfg.cmpnstr[Y_AXIS] = FTM_DEFAULT_Y_COMPENSATOR); - #if HAS_Y_AXIS - cfg.shaper[Y_AXIS] = FTM_DEFAULT_SHAPER_Y; - cfg.baseFreq[Y_AXIS] = FTM_SHAPING_DEFAULT_Y_FREQ; - cfg.zeta[Y_AXIS] = FTM_SHAPING_ZETA_Y; - cfg.vtol[Y_AXIS] = FTM_SHAPING_V_TOL_Y; - #endif + TERN_(HAS_X_AXIS, cfg.baseFreq[X_AXIS] = FTM_SHAPING_DEFAULT_X_FREQ); + TERN_(HAS_Y_AXIS, cfg.baseFreq[Y_AXIS] = FTM_SHAPING_DEFAULT_Y_FREQ); - #if HAS_DYNAMIC_FREQ - cfg.dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE; - cfg.dynFreqK[X_AXIS] = TERN_(HAS_Y_AXIS, cfg.dynFreqK[Y_AXIS]) = 0.0f; - #endif + TERN_(HAS_X_AXIS, cfg.zeta[X_AXIS] = FTM_SHAPING_DEFAULT_ZETA_X); + TERN_(HAS_Y_AXIS, cfg.zeta[Y_AXIS] = FTM_SHAPING_DEFAULT_ZETA_Y); + + TERN_(HAS_X_AXIS, cfg.vtol[X_AXIS] = FTM_SHAPING_DEFAULT_V_TOL_X); + TERN_(HAS_Y_AXIS, cfg.vtol[Y_AXIS] = FTM_SHAPING_DEFAULT_V_TOL_Y); #if HAS_EXTRUDERS cfg.linearAdvEna = FTM_LINEAR_ADV_DEFAULT_ENA; cfg.linearAdvK = FTM_LINEAR_ADV_DEFAULT_K; #endif - TERN_(HAS_X_AXIS, update_shaping_params()); + #if HAS_X_AXIS + update_shaping_params(); + #endif reset(); } @@ -109,17 +99,25 @@ class FTMotion { static bool sts_stepperBusy; // The stepper buffer has items and is in use. + static millis_t axis_pos_move_end_ti[NUM_AXIS_ENUMS], + axis_neg_move_end_ti[NUM_AXIS_ENUMS]; + // Public methods static void init(); static void loop(); // Controller main, to be invoked from non-isr task. #if HAS_X_AXIS - // Refresh gains and indices used by shaping functions. + // Refreshes the gains and indices used by shaping functions. static void update_shaping_params(void); #endif static void reset(); // Reset all states of the fixed time conversion to defaults. + static void setup_traj_gen(uint32_t intervals); + + static bool axis_moving_pos(const AxisEnum axis) { return !ELAPSED(millis(), axis_pos_move_end_ti[axis]); } + static bool axis_moving_neg(const AxisEnum axis) { return !ELAPSED(millis(), axis_neg_move_end_ti[axis]); } + private: static xyze_trajectory_t traj; @@ -162,8 +160,8 @@ class FTMotion { uint32_t Ni[5]; // Shaping time index vector. uint32_t max_i; // Vector length for the selected shaper. - void set_axis_shaping_N(const ftMotionShaper_t shaper, const_float_t f, const_float_t zeta); // Sets the gains used by shaping functions. - void set_axis_shaping_A(const ftMotionShaper_t shaper, const_float_t zeta, const_float_t vtol); // Sets the indices used by shaping functions. + void set_axis_shaping_N(const ftMotionCmpnstr_t shaper, const_float_t f, const_float_t zeta); // Sets the gains used by shaping functions. + void set_axis_shaping_A(const ftMotionCmpnstr_t shaper, const_float_t zeta, const_float_t vtol); // Sets the indices used by shaping functions. } axis_shaping_t; diff --git a/Marlin/src/module/ft_types.h b/Marlin/src/module/ft_types.h index 51cd548..c06f1b6 100644 --- a/Marlin/src/module/ft_types.h +++ b/Marlin/src/module/ft_types.h @@ -23,6 +23,53 @@ #include "../core/types.h" +// FBS-related +typedef enum FXDTICtrlMode : uint8_t { + ftMotionMode_DISABLED = 0, // Standard Motion + ftMotionMode_ENABLED = 1 // Time-Based Motion +} ftMotionMode_t; + +typedef enum FXDTICtrlCmpnstr : uint8_t { + ftMotionCmpnstr_NONE = 0, // No compensator + ftMotionCmpnstr_ZV = 1, // Zero Vibration + ftMotionCmpnstr_ZVD = 2, // Zero Vibration and Derivative + ftMotionCmpnstr_ZVDD = 3, // Zero Vibration, Derivative, and Double Derivative + ftMotionCmpnstr_ZVDDD = 4, // Zero Vibration, Derivative, Double Derivative, and Triple Derivative + ftMotionCmpnstr_EI = 5, // Extra-Intensive + ftMotionCmpnstr_2HEI = 6, // 2-Hump Extra-Intensive + ftMotionCmpnstr_3HEI = 7, // 3-Hump Extra-Intensive + ftMotionCmpnstr_MZV = 8 // Modified Zero Vibration +} ftMotionCmpnstr_t; + +typedef enum FXDTICtrlTrajGenMode : uint8_t { + trajGenMode_NONE = 0U, + trajGenMode_SWEEPC_X = 1U, + trajGenMode_SWEEPC_Y = 2U, + trajGenMode_ABORT = 99U, +} ftMotionTrajGenMode_t; + +typedef struct FXDTICtrlTrajGenConfig { + ftMotionTrajGenMode_t mode = trajGenMode_NONE; + float f0 = 0.0f, + f1 = 0.0f, + dfdt = 0.0f, + a = 0.0f, + pcws_ti[6] = {0.0f}, + k1 = 0.0f, + k2 = 0.0f, + step_ti = 0.0f, + step_a = 0.0f, + dly1_ti = 0.0f, + dly2_ti = 0.0f, + dly3_ti = 0.0f, + step_a_x_0p5 = 0.0f, + step_a_x_step_ti_x_step_ti = 0.0f, + step_ti_x_2 = 0.0f, + step_ti_x_3 = 0.0f, + step_ti_x_4 = 0.0f; +} ftMotionTrajGenConfig_t; +// --- + typedef enum FXDTICtrlShaper : uint8_t { ftMotionShaper_NONE = 0, // No compensator ftMotionShaper_ZV = 1, // Zero Vibration @@ -42,7 +89,7 @@ enum dynFreqMode_t : uint8_t { }; #define CMPNSTR_HAS_SHAPER(A) (ftMotion.cfg.shaper[A] != ftMotionShaper_NONE) -#define CMPNSTR_IS_EISHAPER(A) WITHIN(ftMotion.cfg.shaper[A], ftMotionShaper_EI, ftMotionShaper_3HEI) +//#define CMPNSTR_IS_EISHAPER(A) WITHIN(ftMotion.cfg.shaper[A], ftMotionShaper_EI, ftMotionShaper_3HEI) typedef struct XYZEarray xyze_trajectory_t; typedef struct XYZEarray xyze_trajectoryMod_t; diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 7c746fa..b3dc856 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -1584,7 +1584,7 @@ void Planner::quick_stop() { // Restart the block delay for the first movement - As the queue was // forced to empty, there's no risk the ISR will touch this. - delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.active ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE; + delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE; TERN_(HAS_WIRED_LCD, clear_block_buffer_runtime()); // Clear the accumulated runtime @@ -1745,7 +1745,7 @@ bool Planner::_buffer_steps(const xyze_long_t &target // As there are no queued movements, the Stepper ISR will not touch this // variable, so there is no risk setting this here (but it MUST be done // before the following line!!) - delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.active ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE; + delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE; } // Move buffer head @@ -2804,7 +2804,7 @@ void Planner::buffer_sync_block(const BlockFlagBit sync_flag/*=BLOCK_BIT_SYNC_PO // As there are no queued movements, the Stepper ISR will not touch this // variable, so there is no risk setting this here (but it MUST be done // before the following line!!) - delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.active ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE; + delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE; } block_buffer_head = next_buffer_head; @@ -3097,7 +3097,7 @@ bool Planner::buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s // As there are no queued movements, the Stepper ISR will not touch this // variable, so there is no risk setting this here (but it MUST be done // before the following line!!) - delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.active ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE; + delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE; } // Move buffer head @@ -3118,7 +3118,7 @@ bool Planner::buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s void Planner::set_machine_position_mm(const abce_pos_t &abce) { // When FT Motion is enabled, call synchronize() here instead of generating a sync block - if (TERN0(FT_MOTION, ftMotion.cfg.active)) synchronize(); + if (TERN0(FT_MOTION, ftMotion.cfg.mode)) synchronize(); TERN_(DISTINCT_E_FACTORS, last_extruder = active_extruder); TERN_(HAS_POSITION_FLOAT, position_float = abce); @@ -3153,14 +3153,6 @@ void Planner::set_machine_position_mm(const abce_pos_t &abce) { } } -/** - * Set the machine positions in millimeters (soon) given the native position. - * Synchonized with the planner queue. - * - * - Modifiers are applied for skew, leveling, retract, etc. - * - Kinematics are applied to remap cartesian positions to stepper positions. - * - The resulting stepper positions are synchronized at the end of the planner queue. - */ void Planner::set_position_mm(const xyze_pos_t &xyze) { xyze_pos_t machine = xyze; TERN_(HAS_POSITION_MODIFIERS, apply_modifiers(machine, true)); @@ -3177,13 +3169,12 @@ void Planner::set_position_mm(const xyze_pos_t &xyze) { #if HAS_EXTRUDERS /** - * Special setter for planner E position (also setting E stepper position). + * Setters for planner position (also setting stepper position). */ void Planner::set_e_position_mm(const_float_t e) { const uint8_t axis_index = E_AXIS_N(active_extruder); TERN_(DISTINCT_E_FACTORS, last_extruder = active_extruder); - // Unapply the current retraction before (immediately) setting the planner position const float e_new = DIFF_TERN(FWRETRACT, e, fwretract.current_retract[active_extruder]); position.e = LROUND(settings.axis_steps_per_mm[axis_index] * e_new); TERN_(HAS_POSITION_FLOAT, position_float.e = e_new); @@ -3192,14 +3183,12 @@ void Planner::set_position_mm(const xyze_pos_t &xyze) { if (has_blocks_queued()) buffer_sync_block(BLOCK_BIT_SYNC_POSITION); else - stepper.set_e_position(position.e); + stepper.set_axis_position(E_AXIS, position.e); } -#endif // HAS_EXTRUDERS +#endif -/** - * Recalculate steps/s^2 acceleration rates based on mm/s^2 acceleration rates - */ +// Recalculate the steps/s^2 acceleration rates, based on the mm/s^2 void Planner::refresh_acceleration_rates() { uint32_t highest_rate = 1; LOOP_DISTINCT_AXES(i) { diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index bc658e5..e976574 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1531,7 +1531,8 @@ void Stepper::isr() { uint8_t max_loops = 10; #if ENABLED(FT_MOTION) - const bool using_ftMotion = ftMotion.cfg.active; + const bool using_ftMotion = ftMotion.cfg.mode; + static uint32_t ftMotion_nextAuxISR = 0U; // Storage for the next ISR of the auxilliary tasks. #else constexpr bool using_ftMotion = false; #endif @@ -1550,22 +1551,16 @@ void Stepper::isr() { if (!nextMainISR) { // Main ISR is ready to fire during this iteration? nextMainISR = FTM_MIN_TICKS; // Set to minimum interval (a limit on the top speed) ftMotion_stepper(); // Run FTM Stepping - } - - #if ENABLED(BABYSTEPPING) - if (nextBabystepISR == 0) { // Avoid ANY stepping too soon after baby-stepping - nextBabystepISR = babystepping_isr(); - NOLESS(nextMainISR, (BABYSTEP_TICKS) / 8); // FULL STOP for 125µs after a baby-step + // Define 2.5 msec task for auxilliary functions. + if (!ftMotion_nextAuxISR) { + TERN_(BABYSTEPPING, if (babystep.has_steps()) babystepping_isr()); + ftMotion_nextAuxISR = 0.0025f * (STEPPER_TIMER_RATE); } - if (nextBabystepISR != BABYSTEP_NEVER) // Avoid baby-stepping too close to axis Stepping - NOLESS(nextBabystepISR, nextMainISR / 2); // TODO: Only look at axes enabled for baby-stepping - #endif - - interval = nextMainISR; // Interval is either some old nextMainISR or FTM_MIN_TICKS - TERN_(BABYSTEPPING, NOMORE(interval, nextBabystepISR)); // Come back early for Babystepping? - - nextMainISR = 0; // For FT Motion fire again ASAP - } + } + interval = _MIN(nextMainISR, ftMotion_nextAuxISR); + nextMainISR -= interval; + ftMotion_nextAuxISR -= interval; + } #endif @@ -3361,40 +3356,40 @@ void Stepper::_set_position(const abce_long_t &spos) { #endif } -// AVR requires guards to ensure any atomic memory operation greater than 8 bits -#define ATOMIC_SECTION_START() const bool was_enabled = suspend() -#define ATOMIC_SECTION_END() if (was_enabled) wake_up() -#define AVR_ATOMIC_SECTION_START() TERN_(__AVR__, ATOMIC_SECTION_START()) -#define AVR_ATOMIC_SECTION_END() TERN_(__AVR__, ATOMIC_SECTION_END()) - /** * Get a stepper's position in steps. */ int32_t Stepper::position(const AxisEnum axis) { - AVR_ATOMIC_SECTION_START(); + #ifdef __AVR__ + // Protect the access to the position. Only required for AVR, as + // any 32bit CPU offers atomic access to 32bit variables + const bool was_enabled = suspend(); + #endif + const int32_t v = count_position[axis]; - AVR_ATOMIC_SECTION_END(); + + #ifdef __AVR__ + // Reenable Stepper ISR + if (was_enabled) wake_up(); + #endif return v; } -/** - * Set all axis stepper positions in steps - */ +// Set the current position in steps void Stepper::set_position(const xyze_long_t &spos) { planner.synchronize(); - ATOMIC_SECTION_START(); + const bool was_enabled = suspend(); _set_position(spos); - ATOMIC_SECTION_END(); + if (was_enabled) wake_up(); } -/** - * Set a single axis stepper position in steps - */ void Stepper::set_axis_position(const AxisEnum a, const int32_t &v) { planner.synchronize(); - #if ANY(__AVR__, INPUT_SHAPING_X, INPUT_SHAPING_Y, INPUT_SHAPING_Z) - ATOMIC_SECTION_START(); + #ifdef __AVR__ + // Protect the access to the position. Only required for AVR, as + // any 32bit CPU offers atomic access to 32bit variables + const bool was_enabled = suspend(); #endif count_position[a] = v; @@ -3402,47 +3397,43 @@ void Stepper::set_axis_position(const AxisEnum a, const int32_t &v) { TERN_(INPUT_SHAPING_Y, if (a == Y_AXIS) shaping_y.last_block_end_pos = v); TERN_(INPUT_SHAPING_Z, if (a == Z_AXIS) shaping_z.last_block_end_pos = v); - #if ANY(__AVR__, INPUT_SHAPING_X, INPUT_SHAPING_Y, INPUT_SHAPING_Z) - ATOMIC_SECTION_END(); + #ifdef __AVR__ + // Reenable Stepper ISR + if (was_enabled) wake_up(); #endif } -#if HAS_EXTRUDERS - - void Stepper::set_e_position(const int32_t &v) { - planner.synchronize(); - - AVR_ATOMIC_SECTION_START(); - count_position.e = v; - AVR_ATOMIC_SECTION_END(); - } - -#endif // HAS_EXTRUDERS - #if ENABLED(FT_MOTION) void Stepper::ftMotion_syncPosition() { //planner.synchronize(); planner already synchronized in M493 + #ifdef __AVR__ + // Protect the access to the position. Only required for AVR, as + // any 32bit CPU offers atomic access to 32bit variables + const bool was_enabled = suspend(); + #endif + // Update stepper positions from the planner - AVR_ATOMIC_SECTION_START(); count_position = planner.position; - AVR_ATOMIC_SECTION_END(); + + #ifdef __AVR__ + // Reenable Stepper ISR + if (was_enabled) wake_up(); + #endif } #endif // FT_MOTION -/** - * Record stepper positions and discard the rest of the current block - * - * WARNING! This function may be called from ISR context! - * If the Stepper ISR is preempted (e.g., by the endstop ISR) we - * must ensure the move is properly canceled before the ISR resumes. - */ +// Signal endstops were triggered - This function can be called from +// an ISR context (Temperature, Stepper or limits ISR), so we must +// be very careful here. If the interrupt being preempted was the +// Stepper ISR (this CAN happen with the endstop limits ISR) then +// when the stepper ISR resumes, we must be very sure that the movement +// is properly canceled void Stepper::endstop_triggered(const AxisEnum axis) { - ATOMIC_SECTION_START(); // Suspend the Stepper ISR on all platforms - + const bool was_enabled = suspend(); endstops_trigsteps[axis] = ( #if IS_CORE (axis == CORE_AXIS_2 @@ -3465,14 +3456,23 @@ void Stepper::endstop_triggered(const AxisEnum axis) { // Discard the rest of the move if there is a current block quick_stop(); - ATOMIC_SECTION_END(); // Suspend the Stepper ISR on all platforms + if (was_enabled) wake_up(); } -// Return the "triggered" position for an axis (that hit an endstop) int32_t Stepper::triggered_position(const AxisEnum axis) { - AVR_ATOMIC_SECTION_START(); + #ifdef __AVR__ + // Protect the access to the position. Only required for AVR, as + // any 32bit CPU offers atomic access to 32bit variables + const bool was_enabled = suspend(); + #endif + const int32_t v = endstops_trigsteps[axis]; - AVR_ATOMIC_SECTION_END(); + + #ifdef __AVR__ + // Reenable Stepper ISR + if (was_enabled) wake_up(); + #endif + return v; } @@ -3501,30 +3501,35 @@ void Stepper::report_a_position(const xyz_long_t &pos) { } void Stepper::report_positions() { - AVR_ATOMIC_SECTION_START(); + + #ifdef __AVR__ + // Protect the access to the position. + const bool was_enabled = suspend(); + #endif + const xyz_long_t pos = count_position; - AVR_ATOMIC_SECTION_END(); + + #ifdef __AVR__ + if (was_enabled) wake_up(); + #endif + report_a_position(pos); } #if ENABLED(FT_MOTION) - /** - * Run stepping from the Stepper ISR at regular short intervals. - * - * - Set ftMotion.sts_stepperBusy state to reflect whether there are any commands in the circular buffer. - * - If there are no commands in the buffer, return. - * - Get the next command from the circular buffer ftMotion.stepperCmdBuff[]. - * - If the block is being aborted, return without processing the command. - * - Apply STEP/DIR along with any delays required. A command may be empty, with no STEP/DIR. - */ + // Set stepper I/O for fixed time controller. void Stepper::ftMotion_stepper() { + static AxisBits direction_bits{0}; + // Check if the buffer is empty. ftMotion.sts_stepperBusy = (ftMotion.stepperCmdBuff_produceIdx != ftMotion.stepperCmdBuff_consumeIdx); if (!ftMotion.sts_stepperBusy) return; // "Pop" one command from current motion buffer + // Use one byte to restore one stepper command in the format: + // |X_step|X_direction|Y_step|Y_direction|Z_step|Z_direction|E_step|E_direction| const ft_command_t command = ftMotion.stepperCmdBuff[ftMotion.stepperCmdBuff_consumeIdx]; if (++ftMotion.stepperCmdBuff_consumeIdx == (FTM_STEPPERCMD_BUFF_SIZE)) ftMotion.stepperCmdBuff_consumeIdx = 0; @@ -3533,55 +3538,59 @@ void Stepper::report_positions() { USING_TIMED_PULSE(); - // Get FT Motion command flags for axis STEP / DIR - #define _FTM_STEP(AXIS) TEST(command, FT_BIT_STEP_##AXIS) - #define _FTM_DIR(AXIS) TEST(command, FT_BIT_DIR_##AXIS) - - /** - * Set bits in axis_did_move for any axes moving in this block, - * clearing the bits at the start of each new segment. - */ - if (TEST(command, FT_BIT_START)) axis_did_move.reset(); - - #define _FTM_AXIS_DID_MOVE(AXIS) axis_did_move.bset(_AXIS(AXIS), _FTM_STEP(AXIS)); - LOGICAL_AXIS_MAP(_FTM_AXIS_DID_MOVE); - - /** - * Update direction bits for steppers that were stepped by this command. - * HX, HY, HZ direction bits were set for Core kinematics - * when the block was fetched and are not overwritten here. - */ - - #define _FTM_SET_DIR(AXIS) if (_FTM_STEP(AXIS)) last_direction_bits.bset(_AXIS(AXIS), _FTM_DIR(AXIS)); - LOGICAL_AXIS_MAP(_FTM_SET_DIR); + AxisBits axis_step; + axis_step = LOGICAL_AXIS_ARRAY( + TEST(command, FT_BIT_STEP_E), + TEST(command, FT_BIT_STEP_X), TEST(command, FT_BIT_STEP_Y), TEST(command, FT_BIT_STEP_Z), + TEST(command, FT_BIT_STEP_I), TEST(command, FT_BIT_STEP_J), TEST(command, FT_BIT_STEP_K), + TEST(command, FT_BIT_STEP_U), TEST(command, FT_BIT_STEP_V), TEST(command, FT_BIT_STEP_W) + ); - if (TERN1(FTM_OPTIMIZE_DIR_STATES, last_set_direction != last_direction_bits)) { - // Apply directions (generally applying to the entire linear move) - #define _FTM_APPLY_DIR(AXIS) if (TERN1(FTM_OPTIMIZE_DIR_STATES, last_direction_bits[_AXIS(A)] != last_set_direction[_AXIS(AXIS)])) \ - SET_STEP_DIR(AXIS); - LOGICAL_AXIS_MAP(_FTM_APPLY_DIR); + direction_bits = LOGICAL_AXIS_ARRAY( + axis_step.e ? TEST(command, FT_BIT_DIR_E) : direction_bits.e, + axis_step.x ? TEST(command, FT_BIT_DIR_X) : direction_bits.x, + axis_step.y ? TEST(command, FT_BIT_DIR_Y) : direction_bits.y, + axis_step.z ? TEST(command, FT_BIT_DIR_Z) : direction_bits.z, + axis_step.i ? TEST(command, FT_BIT_DIR_I) : direction_bits.i, + axis_step.j ? TEST(command, FT_BIT_DIR_J) : direction_bits.j, + axis_step.k ? TEST(command, FT_BIT_DIR_K) : direction_bits.k, + axis_step.u ? TEST(command, FT_BIT_DIR_U) : direction_bits.u, + axis_step.v ? TEST(command, FT_BIT_DIR_V) : direction_bits.v, + axis_step.w ? TEST(command, FT_BIT_DIR_W) : direction_bits.w + ); - TERN_(FTM_OPTIMIZE_DIR_STATES, last_set_direction = last_direction_bits); + // Apply directions (which will apply to the entire linear move) + LOGICAL_AXIS_CODE( + E_APPLY_DIR(direction_bits.e, false), + X_APPLY_DIR(direction_bits.x, false), Y_APPLY_DIR(direction_bits.y, false), Z_APPLY_DIR(direction_bits.z, false), + I_APPLY_DIR(direction_bits.i, false), J_APPLY_DIR(direction_bits.j, false), K_APPLY_DIR(direction_bits.k, false), + U_APPLY_DIR(direction_bits.u, false), V_APPLY_DIR(direction_bits.v, false), W_APPLY_DIR(direction_bits.w, false) + ); - // Any DIR change requires a wait period - DIR_WAIT_AFTER(); - } + DIR_WAIT_AFTER(); - // Start step pulses. Edge stepping will toggle the STEP pin. - #define _FTM_STEP_START(AXIS) AXIS##_APPLY_STEP(_FTM_STEP(AXIS), false); - LOGICAL_AXIS_MAP(_FTM_STEP_START); + // Start a step pulse + LOGICAL_AXIS_CODE( + E_APPLY_STEP(axis_step.e, false), + X_APPLY_STEP(axis_step.x, false), Y_APPLY_STEP(axis_step.y, false), Z_APPLY_STEP(axis_step.z, false), + I_APPLY_STEP(axis_step.i, false), J_APPLY_STEP(axis_step.j, false), K_APPLY_STEP(axis_step.k, false), + U_APPLY_STEP(axis_step.u, false), V_APPLY_STEP(axis_step.v, false), W_APPLY_STEP(axis_step.w, false) + ); - // Apply steps via I2S TERN_(I2S_STEPPER_STREAM, i2s_push_sample()); // Begin waiting for the minimum pulse duration START_TIMED_PULSE(); // Update step counts - #define _FTM_STEP_COUNT(AXIS) if (_FTM_STEP(AXIS)) count_position[_AXIS(AXIS)] += last_direction_bits[_AXIS(AXIS)] ? 1 : -1; - LOGICAL_AXIS_MAP(_FTM_STEP_COUNT); + LOGICAL_AXIS_CODE( + if (axis_step.e) count_position.e += direction_bits.e ? 1 : -1, if (axis_step.x) count_position.x += direction_bits.x ? 1 : -1, + if (axis_step.y) count_position.y += direction_bits.y ? 1 : -1, if (axis_step.z) count_position.z += direction_bits.z ? 1 : -1, + if (axis_step.i) count_position.i += direction_bits.i ? 1 : -1, if (axis_step.j) count_position.j += direction_bits.j ? 1 : -1, + if (axis_step.k) count_position.k += direction_bits.k ? 1 : -1, if (axis_step.u) count_position.u += direction_bits.u ? 1 : -1, + if (axis_step.v) count_position.v += direction_bits.v ? 1 : -1, if (axis_step.w) count_position.w += direction_bits.w ? 1 : -1 + ); - // Provide EDGE flags for E stepper(s) #if HAS_EXTRUDERS #if ENABLED(E_DUAL_STEPPER_DRIVERS) constexpr bool e_axis_has_dedge = AXIS_HAS_DEDGE(E0) && AXIS_HAS_DEDGE(E1); @@ -3594,23 +3603,22 @@ void Stepper::report_positions() { // Only wait for axes without edge stepping const bool any_wait = false LOGICAL_AXIS_GANG( - || (!e_axis_has_dedge && _FTM_STEP(E)), - || (!AXIS_HAS_DEDGE(X) && _FTM_STEP(X)), || (!AXIS_HAS_DEDGE(Y) && _FTM_STEP(Y)), || (!AXIS_HAS_DEDGE(Z) && _FTM_STEP(Z)), - || (!AXIS_HAS_DEDGE(I) && _FTM_STEP(I)), || (!AXIS_HAS_DEDGE(J) && _FTM_STEP(J)), || (!AXIS_HAS_DEDGE(K) && _FTM_STEP(K)), - || (!AXIS_HAS_DEDGE(U) && _FTM_STEP(U)), || (!AXIS_HAS_DEDGE(V) && _FTM_STEP(V)), || (!AXIS_HAS_DEDGE(W) && _FTM_STEP(W)) + || (!e_axis_has_dedge && axis_step.e), + || (!AXIS_HAS_DEDGE(X) && axis_step.x), || (!AXIS_HAS_DEDGE(Y) && axis_step.y), || (!AXIS_HAS_DEDGE(Z) && axis_step.z), + || (!AXIS_HAS_DEDGE(I) && axis_step.i), || (!AXIS_HAS_DEDGE(J) && axis_step.j), || (!AXIS_HAS_DEDGE(K) && axis_step.k), + || (!AXIS_HAS_DEDGE(U) && axis_step.u), || (!AXIS_HAS_DEDGE(V) && axis_step.v), || (!AXIS_HAS_DEDGE(W) && axis_step.w) ); // Allow pulses to be registered by stepper drivers if (any_wait) AWAIT_HIGH_PULSE(); // Stop pulses. Axes with DEDGE will do nothing, assuming STEP_STATE_* is HIGH - #define _FTM_STEP_STOP(AXIS) AXIS##_APPLY_STEP(!STEP_STATE_##AXIS, false); - LOGICAL_AXIS_MAP(_FTM_STEP_STOP); - - // Check endstops on every step using axis_did_move as set by every step - // TODO: Update endstop states less frequently to save processing. - // NOTE: endstops.poll is still called at 1KHz by Temperature ISR. - IF_DISABLED(ENDSTOP_INTERRUPTS_FEATURE, if ((bool)axis_did_move) endstops.update()); + LOGICAL_AXIS_CODE( + E_APPLY_STEP(!STEP_STATE_E, false), + X_APPLY_STEP(!STEP_STATE_X, false), Y_APPLY_STEP(!STEP_STATE_Y, false), Z_APPLY_STEP(!STEP_STATE_Z, false), + I_APPLY_STEP(!STEP_STATE_I, false), J_APPLY_STEP(!STEP_STATE_J, false), K_APPLY_STEP(!STEP_STATE_K, false), + U_APPLY_STEP(!STEP_STATE_U, false), V_APPLY_STEP(!STEP_STATE_V, false), W_APPLY_STEP(!STEP_STATE_W, false) + ); } // Stepper::ftMotion_stepper diff --git a/platformio.ini b/platformio.ini index 76200cb..a1e1957 100644 --- a/platformio.ini +++ b/platformio.ini @@ -13,7 +13,7 @@ [platformio] src_dir = Marlin boards_dir = buildroot/share/PlatformIO/boards -default_envs = mega2560 +default_envs = STM32F103RE_creality include_dir = Marlin extra_configs = Marlin/config.ini @@ -44,7 +44,7 @@ extra_configs = # Remove '-fmax-errors=5' from build_flags below to see all. # [common] -build_flags = -g3 -D__MARLIN_FIRMWARE__ -DNDEBUG +build_flags = -O3 -D__MARLIN_FIRMWARE__ -DNDEBUG -fmax-errors=5 extra_scripts = pre:buildroot/share/PlatformIO/scripts/configuration.py