0x90/Marlin
12
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

✨ Fixed-Time Motion with Input Shaping by Ulendo (#25394)

Browse source 
Co-authored-by: Ulendo Alex <alex@ulendo.io>
This commit is contained in:
Scott Lahteine 2023-03-31 21:18:37 -05:00 committed by GitHub
parent 8cdf43f8fd
commit c37fa3cc90
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
15 changed files with 1772 additions and 50 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

48
Marlin/Configuration_adv.h
View file

@ -1086,7 +1086,51 @@
#endif
// @section motion
// @section motion control
/**
* Fixed-time-based Motion Control -- EXPERIMENTAL
* Enable/disable and set parameters with G-code M493.
*/
//#define FT_MOTION
#if ENABLED(FT_MOTION)
#define FTM_DEFAULT_MODE ftMotionMode_ENABLED // Default mode of fixed time control. (Enums in ft_types.h)
#define FTM_DEFAULT_DYNFREQ_MODE dynFreqMode_DISABLED // Default mode of dynamic frequency calculation. (Enums in ft_types.h)
#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.0f // Default linear advance gain.
#define FTM_SHAPING_ZETA 0.1f // Zeta used by input shapers.
#define FTM_SHAPING_V_TOL 0.05f // Vibration tolerance used by EI input shapers.
/**
* Advanced configuration
*/
#define FTM_BATCH_SIZE 100 // Batch size for trajectory generation;
// half the window size for Ulendo FBS.
#define FTM_FS 1000 // (Hz) Frequency for trajectory generation. (1 / FTM_TS)
#define FTM_TS 0.001f // (s) Time step for trajectory generation. (1 / FTM_FS)
#define FTM_STEPPER_FS 20000 // (Hz) Frequency for stepper I/O update.
#define FTM_MIN_TICKS ((STEPPER_TIMER_RATE) / (FTM_STEPPER_FS)) // Minimum stepper ticks between steps.
#define FTM_MIN_SHAPE_FREQ 10 // Minimum shaping frequency.
#define FTM_ZMAX 100 // Maximum delays for shaping functions (even numbers only!).
// Calculate as:
// 1/2 * (FTM_FS / FTM_MIN_SHAPE_FREQ) for ZV.
// (FTM_FS / FTM_MIN_SHAPE_FREQ) for ZVD, MZV.
// 3/2 * (FTM_FS / FTM_MIN_SHAPE_FREQ) for 2HEI.
// 2 * (FTM_FS / FTM_MIN_SHAPE_FREQ) for 3HEI.
#define FTM_STEPS_PER_UNIT_TIME 20 // Interpolated stepper commands per unit time.
// Calculate as (FTM_STEPPER_FS / FTM_FS).
#define FTM_CTS_COMPARE_VAL 10 // Comparison value used in interpolation algorithm.
// Calculate as (FTM_STEPS_PER_UNIT_TIME / 2).
// These values may be configured to adjust 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().
// This value may be configured to adjust duration to consume the command buffer.
// Try increasing this value if stepper motion is not smooth.
#define FTM_STEPPERCMD_BUFF_SIZE 1000 // Size of the stepper command buffers.
#endif
/**
* Input Shaping -- EXPERIMENTAL
@ -1125,6 +1169,8 @@
//#define SHAPING_MENU // Add a menu to the LCD to set shaping parameters.
#endif
// @section motion
#define AXIS_RELATIVE_MODES { false, false, false, false }
// Add a Duplicate option for well-separated conjoined nozzles

7
Marlin/src/MarlinCore.cpp
View file

@ -50,6 +50,9 @@
#include "module/settings.h"
#include "module/stepper.h"
#include "module/temperature.h"
#if ENABLED(FT_MOTION)
#include "module/ft_motion.h"
#endif
#include "gcode/gcode.h"
#include "gcode/parser.h"
@ -885,8 +888,12 @@ void idle(bool no_stepper_sleep/*=false*/) {
// Update the LVGL interface
TERN_(HAS_TFT_LVGL_UI, LV_TASK_HANDLER());
// Manage Fixed-time Motion Control
TERN_(FT_MOTION, fxdTiCtrl.loop());
IDLE_DONE:
TERN_(MARLIN_DEV_MODE, idle_depth--);
return;
}

282
Marlin/src/gcode/feature/ft_motion/M493.cpp Normal file
View file

@ -0,0 +1,282 @@
/**
* 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 <https://www.gnu.org/licenses/>.
*
*/
#include "../../../inc/MarlinConfig.h"
#if ENABLED(FT_MOTION)
#include "../../gcode.h"
#include "../../../module/ft_motion.h"
void say_shaping() {
SERIAL_ECHO_TERNARY(fxdTiCtrl.cfg_mode, "Fixed time controller ", "en", "dis", "abled");
if (fxdTiCtrl.cfg_mode == ftMotionMode_DISABLED || fxdTiCtrl.cfg_mode == ftMotionMode_ENABLED) {
SERIAL_ECHOLNPGM(".");
return;
}
#if HAS_X_AXIS
SERIAL_ECHOPGM(" with ");
switch (fxdTiCtrl.cfg_mode) {
default: break;
//case ftMotionMode_ULENDO_FBS: SERIAL_ECHOLNPGM("Ulendo FBS."); return;
case ftMotionMode_ZV: SERIAL_ECHOLNPGM("ZV"); break;
case ftMotionMode_ZVD: SERIAL_ECHOLNPGM("ZVD"); break;
case ftMotionMode_EI: SERIAL_ECHOLNPGM("EI"); break;
case ftMotionMode_2HEI: SERIAL_ECHOLNPGM("2 Hump EI"); break;
case ftMotionMode_3HEI: SERIAL_ECHOLNPGM("3 Hump EI"); break;
case ftMotionMode_MZV: SERIAL_ECHOLNPGM("MZV"); break;
//case ftMotionMode_DISCTF: SERIAL_ECHOLNPGM("discrete transfer functions"); break;
}
SERIAL_ECHOLNPGM(" shaping.");
#endif
}
/**
* M493: Set Fixed-time Motion Control parameters
*
* S<mode> Set the motion / shaping mode. Shaping requires an X axis, at the minimum.
* 0: NORMAL
* 1: FIXED-TIME
* 10: ZV
* 11: ZVD
* 12: EI
* 13: 2HEI
* 14: 3HEI
* 15: MZV
*
* P<bool> Enable (1) or Disable (0) Linear Advance pressure control
*
* K<gain> Set Linear Advance gain
*
* D<mode> Set Dynamic Frequency mode
* 0: DISABLED
* 1: Z-based (Requires a Z axis)
* 2: Mass-based (Requires X and E axes)
*
* A<Hz> Set static/base frequency for the X axis
* F<Hz> Set frequency scaling for the X axis
*
* B<Hz> Set static/base frequency for the Y axis
* H<Hz> Set frequency scaling for the Y axis
*/
void GcodeSuite::M493() {
// Parse 'S' mode parameter.
if (parser.seenval('S')) {
const ftMotionMode_t val = (ftMotionMode_t)parser.value_byte();
switch (val) {
case ftMotionMode_DISABLED:
case ftMotionMode_ENABLED:
#if HAS_X_AXIS
case ftMotionMode_ZVD:
case ftMotionMode_2HEI:
case ftMotionMode_3HEI:
case ftMotionMode_MZV:
//case ftMotionMode_ULENDO_FBS:
//case ftMotionMode_DISCTF:
fxdTiCtrl.cfg_mode = val;
say_shaping();
break;
#endif
default:
SERIAL_ECHOLNPGM("?Invalid control mode [M] value.");
return;
}
switch (val) {
case ftMotionMode_ENABLED: fxdTiCtrl.reset(); break;
#if HAS_X_AXIS
case ftMotionMode_ZV:
case ftMotionMode_ZVD:
case ftMotionMode_EI:
case ftMotionMode_2HEI:
case ftMotionMode_3HEI:
case ftMotionMode_MZV:
fxdTiCtrl.updateShapingN(fxdTiCtrl.cfg_baseFreq[0] OPTARG(HAS_Y_AXIS, fxdTiCtrl.cfg_baseFreq[1]));
fxdTiCtrl.updateShapingA();
fxdTiCtrl.reset();
break;
//case ftMotionMode_ULENDO_FBS:
//case ftMotionMode_DISCTF:
#endif
default: break;
}
}
#if HAS_EXTRUDERS
// Pressure control (linear advance) parameter.
if (parser.seen('P')) {
const bool val = parser.value_bool();
fxdTiCtrl.cfg_linearAdvEna = val;
SERIAL_ECHO_TERNARY(val, "Pressure control: Linear Advance ", "en", "dis", "abled.\n");
}
// Pressure control (linear advance) gain parameter.
if (parser.seenval('K')) {
const float val = parser.value_float();
if (val >= 0.0f) {
fxdTiCtrl.cfg_linearAdvK = val;
SERIAL_ECHOPGM("Pressure control: Linear Advance gain set to: ");
SERIAL_ECHO_F(val, 5);
SERIAL_ECHOLNPGM(".");
}
else { // Value out of range.
SERIAL_ECHOLNPGM("Pressure control: Linear Advance gain out of range.");
}
}
#endif // HAS_EXTRUDERS
#if HAS_Z_AXIS || HAS_EXTRUDERS
// Dynamic frequency mode parameter.
if (parser.seenval('D')) {
if (WITHIN(fxdTiCtrl.cfg_mode, 10U, 19U)) {
const dynFreqMode_t val = dynFreqMode_t(parser.value_byte());
switch (val) {
case dynFreqMode_DISABLED:
fxdTiCtrl.cfg_dynFreqMode = val;
SERIAL_ECHOLNPGM("Dynamic frequency mode disabled.");
break;
#if HAS_Z_AXIS
case dynFreqMode_Z_BASED:
fxdTiCtrl.cfg_dynFreqMode = val;
SERIAL_ECHOLNPGM("Z-based Dynamic Frequency Mode.");
break;
#endif
#if HAS_EXTRUDERS
case dynFreqMode_MASS_BASED:
fxdTiCtrl.cfg_dynFreqMode = val;
SERIAL_ECHOLNPGM("Mass-based Dynamic Frequency Mode.");
break;
#endif
default:
SERIAL_ECHOLNPGM("?Invalid Dynamic Frequency Mode [D] value.");
break;
}
}
else {
SERIAL_ECHOLNPGM("Incompatible shaper for [D] Dynamic Frequency mode.");
}
}
#endif // HAS_Z_AXIS || HAS_EXTRUDERS
#if HAS_X_AXIS
// Parse frequency parameter (X axis).
if (parser.seenval('A')) {
if (WITHIN(fxdTiCtrl.cfg_mode, 10U, 19U)) {
const float val = parser.value_float();
const bool frequencyInRange = WITHIN(val, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2);
// TODO: Frequency minimum is dependent on the shaper used; the above check isn't always correct.
if (frequencyInRange) {
fxdTiCtrl.cfg_baseFreq[0] = val;
fxdTiCtrl.updateShapingN(fxdTiCtrl.cfg_baseFreq[0] OPTARG(HAS_Y_AXIS, fxdTiCtrl.cfg_baseFreq[1]));
fxdTiCtrl.reset();
if (fxdTiCtrl.cfg_dynFreqMode) { SERIAL_ECHOPGM("Compensator base dynamic frequency (X/A axis) set to:"); }
else { SERIAL_ECHOPGM("Compensator static frequency (X/A axis) set to: "); }
SERIAL_ECHO_F( fxdTiCtrl.cfg_baseFreq[0], 2 );
SERIAL_ECHOLNPGM(".");
}
else { // Frequency out of range.
SERIAL_ECHOLNPGM("Invalid [A] frequency value.");
}
}
else { // Mode doesn't use frequency.
SERIAL_ECHOLNPGM("Incompatible mode for [A] frequency.");
}
}
#if HAS_Z_AXIS || HAS_EXTRUDERS
// Parse frequency scaling parameter (X axis).
if (parser.seenval('F')) {
const bool modeUsesDynFreq = (
TERN0(HAS_Z_AXIS, fxdTiCtrl.cfg_dynFreqMode == dynFreqMode_Z_BASED)
|| TERN0(HAS_EXTRUDERS, fxdTiCtrl.cfg_dynFreqMode == dynFreqMode_MASS_BASED)
);
if (modeUsesDynFreq) {
const float val = parser.value_float();
fxdTiCtrl.cfg_dynFreqK[0] = val;
SERIAL_ECHOPGM("Frequency scaling (X/A axis) set to: ");
SERIAL_ECHO_F(fxdTiCtrl.cfg_dynFreqK[0], 8);
SERIAL_ECHOLNPGM(".");
}
else {
SERIAL_ECHOLNPGM("Incompatible mode for [F] frequency scaling.");
}
}
#endif // HAS_Z_AXIS || HAS_EXTRUDERS
#endif // HAS_X_AXIS
#if HAS_Y_AXIS
// Parse frequency parameter (Y axis).
if (parser.seenval('B')) {
if (WITHIN(fxdTiCtrl.cfg_mode, 10U, 19U)) {
const float val = parser.value_float();
const bool frequencyInRange = WITHIN(val, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2);
if (frequencyInRange) {
fxdTiCtrl.cfg_baseFreq[1] = val;
fxdTiCtrl.updateShapingN(fxdTiCtrl.cfg_baseFreq[0] OPTARG(HAS_Y_AXIS, fxdTiCtrl.cfg_baseFreq[1]));
fxdTiCtrl.reset();
if (fxdTiCtrl.cfg_dynFreqMode) { SERIAL_ECHOPGM("Compensator base dynamic frequency (Y/B axis) set to:"); }
else { SERIAL_ECHOPGM("Compensator static frequency (Y/B axis) set to: "); }
SERIAL_ECHO_F( fxdTiCtrl.cfg_baseFreq[1], 2 );
SERIAL_ECHOLNPGM(".");
}
else { // Frequency out of range.
SERIAL_ECHOLNPGM("Invalid frequency [B] value.");
}
}
else { // Mode doesn't use frequency.
SERIAL_ECHOLNPGM("Incompatible mode for [B] frequency.");
}
}
#if HAS_Z_AXIS || HAS_EXTRUDERS
// Parse frequency scaling parameter (Y axis).
if (parser.seenval('H')) {
const bool modeUsesDynFreq = (
TERN0(HAS_Z_AXIS, fxdTiCtrl.cfg_dynFreqMode == dynFreqMode_Z_BASED)
|| TERN0(HAS_EXTRUDERS, fxdTiCtrl.cfg_dynFreqMode == dynFreqMode_MASS_BASED)
);
if (modeUsesDynFreq) {
const float val = parser.value_float();
fxdTiCtrl.cfg_dynFreqK[1] = val;
SERIAL_ECHOPGM("Frequency scaling (Y/B axis) set to: ");
SERIAL_ECHO_F(val, 8);
SERIAL_ECHOLNPGM(".");
}
else {
SERIAL_ECHOLNPGM("Incompatible mode for [H] frequency scaling.");
}
}
#endif // HAS_Z_AXIS || HAS_EXTRUDERS
#endif // HAS_Y_AXIS
}
#endif // FT_MOTION

6
Marlin/src/gcode/gcode.cpp
View file

@ -895,6 +895,10 @@ void GcodeSuite::process_parsed_command(const bool no_ok/*=false*/) {
case 486: M486(); break; // M486: Identify and cancel objects
#endif
#if ENABLED(FT_MOTION)
case 493: M493(); break; // M493: Fixed-Time Motion control
#endif
case 500: M500(); break; // M500: Store settings in EEPROM
case 501: M501(); break; // M501: Read settings from EEPROM
case 502: M502(); break; // M502: Revert to default settings
@ -934,7 +938,7 @@ void GcodeSuite::process_parsed_command(const bool no_ok/*=false*/) {
#endif
#if HAS_ZV_SHAPING
case 593: M593(); break; // M593: Set Input Shaping parameters
case 593: M593(); break; // M593: Input Shaping control
#endif
#if ENABLED(ADVANCED_PAUSE_FEATURE)

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

@ -1038,6 +1038,10 @@ private:
static void M486();
#endif
#if ENABLED(FT_MOTION)
static void M493();
#endif
static void M500();
static void M501();
static void M502();

924
Marlin/src/module/ft_motion.cpp Normal file
View file

@ -0,0 +1,924 @@
/**
* 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 <https://www.gnu.org/licenses/>.
*
*/
#include "../inc/MarlinConfig.h"
#if ENABLED(FT_MOTION)
#include "ft_motion.h"
#include "stepper.h" // Access stepper block queue function and abort status.
FxdTiCtrl fxdTiCtrl;
//-----------------------------------------------------------------//
// Variables.
//-----------------------------------------------------------------//
// Public variables.
ftMotionMode_t FxdTiCtrl::cfg_mode = FTM_DEFAULT_MODE; // Mode / active compensation mode configuration.
#if HAS_EXTRUDERS
bool FxdTiCtrl::cfg_linearAdvEna = FTM_LINEAR_ADV_DEFAULT_ENA; // Linear advance enable configuration.
float FxdTiCtrl::cfg_linearAdvK = FTM_LINEAR_ADV_DEFAULT_K; // Linear advance gain.
#endif
dynFreqMode_t FxdTiCtrl::cfg_dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE; // Dynamic frequency mode configuration.
#if !HAS_Z_AXIS
static_assert(FTM_DEFAULT_DYNFREQ_MODE != dynFreqMode_Z_BASED, "dynFreqMode_Z_BASED requires a Z axis.");
#endif
#if !(HAS_X_AXIS && HAS_EXTRUDERS)
static_assert(FTM_DEFAULT_DYNFREQ_MODE != dynFreqMode_MASS_BASED, "dynFreqMode_MASS_BASED requires an X axis and an extruder.");
#endif
#if HAS_X_AXIS
float FxdTiCtrl::cfg_baseFreq[] = { FTM_SHAPING_DEFAULT_X_FREQ // Base frequency. [Hz]
OPTARG(HAS_Y_AXIS, FTM_SHAPING_DEFAULT_Y_FREQ) };
float FxdTiCtrl::cfg_dynFreqK[] = { 0.0f OPTARG(HAS_Y_AXIS, 0.0f) }; // Scaling / gain for dynamic frequency. [Hz/mm] or [Hz/g]
#endif
ft_command_t FxdTiCtrl::stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE] = {0U}; // Buffer of stepper commands.
hal_timer_t FxdTiCtrl::stepperCmdBuff_StepRelativeTi[FTM_STEPPERCMD_BUFF_SIZE] = {0U}; // Buffer of the stepper command timing.
uint8_t FxdTiCtrl::stepperCmdBuff_ApplyDir[FTM_STEPPERCMD_DIR_SIZE] = {0U}; // Buffer of whether DIR needs to be updated.
uint32_t FxdTiCtrl::stepperCmdBuff_produceIdx = 0, // Index of next stepper command write to the buffer.
FxdTiCtrl::stepperCmdBuff_consumeIdx = 0; // Index of next stepper command read from the buffer.
bool FxdTiCtrl::sts_stepperBusy = false; // The stepper buffer has items and is in use.
// Private variables.
// NOTE: These are sized for Ulendo FBS use.
#if HAS_X_AXIS
float FxdTiCtrl::xd[2 * (FTM_BATCH_SIZE)], // = {0.0f} Storage for fixed-time-based trajectory.
FxdTiCtrl::xm[FTM_BATCH_SIZE]; // = {0.0f} Storage for modified fixed-time-based trajectory.
#endif
#if HAS_Y_AXIS
float FxdTiCtrl::yd[2 * (FTM_BATCH_SIZE)], FxdTiCtrl::ym[FTM_BATCH_SIZE];
#endif
#if HAS_Z_AXIS
float FxdTiCtrl::zd[2 * (FTM_BATCH_SIZE)], FxdTiCtrl::zm[FTM_BATCH_SIZE];
#endif
#if HAS_EXTRUDERS
float FxdTiCtrl::ed[2 * (FTM_BATCH_SIZE)], FxdTiCtrl::em[FTM_BATCH_SIZE];
#endif
block_t* FxdTiCtrl::current_block_cpy = nullptr; // Pointer to current block being processed.
bool FxdTiCtrl::blockProcRdy = false, // Indicates a block is ready to be processed.
FxdTiCtrl::blockProcRdy_z1 = false, // Storage for the previous indicator.
FxdTiCtrl::blockProcDn = false; // Indicates current block is done being processed.
bool FxdTiCtrl::batchRdy = false; // Indicates a batch of the fixed time trajectory
// has been generated, is now available in the upper -
// half of xd, yd, zd, ed vectors, and is ready to be
// post processed, if applicable, then interpolated.
bool FxdTiCtrl::batchRdyForInterp = false; // Indicates the batch is done being post processed,
// if applicable, and is ready to be converted to step commands.
bool FxdTiCtrl::runoutEna = false; // True if runout of the block hasn't been done and is allowed.
// Trapezoid data variables.
#if HAS_X_AXIS
float FxdTiCtrl::x_startPosn, // (mm) Start position of block
FxdTiCtrl::x_endPosn_prevBlock = 0.0f, // (mm) Start position of block
FxdTiCtrl::x_Ratio; // (ratio) Axis move ratio of block
#endif
#if HAS_Y_AXIS
float FxdTiCtrl::y_startPosn,
FxdTiCtrl::y_endPosn_prevBlock = 0.0f,
FxdTiCtrl::y_Ratio;
#endif
#if HAS_Z_AXIS
float FxdTiCtrl::z_startPosn,
FxdTiCtrl::z_endPosn_prevBlock = 0.0f,
FxdTiCtrl::z_Ratio;
#endif
#if HAS_EXTRUDERS
float FxdTiCtrl::e_startPosn,
FxdTiCtrl::e_endPosn_prevBlock = 0.0f,
FxdTiCtrl::e_Ratio;
#endif
float FxdTiCtrl::accel_P, // Acceleration prime of block. [mm/sec/sec]
FxdTiCtrl::decel_P, // Deceleration prime of block. [mm/sec/sec]
FxdTiCtrl::F_P, // Feedrate prime of block. [mm/sec]
FxdTiCtrl::f_s, // Starting feedrate of block. [mm/sec]
FxdTiCtrl::s_1e, // Position after acceleration phase of block.
FxdTiCtrl::s_2e; // Position after acceleration and coasting phase of block.
uint32_t FxdTiCtrl::N1, // Number of data points in the acceleration phase.
FxdTiCtrl::N2, // Number of data points in the coasting phase.
FxdTiCtrl::N3; // Number of data points in the deceleration phase.
uint32_t FxdTiCtrl::max_intervals; // Total number of data points that will be generated from block.
// Make vector variables.
uint32_t FxdTiCtrl::makeVector_idx = 0, // Index of fixed time trajectory generation of the overall block.
FxdTiCtrl::makeVector_idx_z1 = 0, // Storage for the previously calculated index above.
FxdTiCtrl::makeVector_batchIdx = FTM_BATCH_SIZE; // Index of fixed time trajectory generation within the batch.
// Interpolation variables.
#if HAS_X_AXIS
int32_t FxdTiCtrl::x_steps = 0; // Step count accumulator.
stepDirState_t FxdTiCtrl::x_dirState = stepDirState_NOT_SET; // Memory of the currently set step direction of the axis.
#endif
#if HAS_Y_AXIS
int32_t FxdTiCtrl::y_steps = 0;
stepDirState_t FxdTiCtrl::y_dirState = stepDirState_NOT_SET;
#endif
#if HAS_Z_AXIS
int32_t FxdTiCtrl::z_steps = 0;
stepDirState_t FxdTiCtrl::z_dirState = stepDirState_NOT_SET;
#endif
#if HAS_EXTRUDERS
int32_t FxdTiCtrl::e_steps = 0;
stepDirState_t FxdTiCtrl::e_dirState = stepDirState_NOT_SET;
#endif
uint32_t FxdTiCtrl::interpIdx = 0, // Index of current data point being interpolated.
FxdTiCtrl::interpIdx_z1 = 0; // Storage for the previously calculated index above.
hal_timer_t FxdTiCtrl::nextStepTicks = FTM_MIN_TICKS; // Accumulator for the next step time (in ticks).
// Shaping variables.
#if HAS_X_AXIS
uint32_t FxdTiCtrl::xy_zi_idx = 0, // Index of storage in the data point delay vectors.
FxdTiCtrl::xy_max_i = 0; // Vector length for the selected shaper.
float FxdTiCtrl::xd_zi[FTM_ZMAX] = { 0.0f }; // Data point delay vector.
float FxdTiCtrl::x_Ai[5]; // Shaping gain vector.
uint32_t FxdTiCtrl::x_Ni[5]; // Shaping time index vector.
#endif
#if HAS_Y_AXIS
float FxdTiCtrl::yd_zi[FTM_ZMAX] = { 0.0f };
float FxdTiCtrl::y_Ai[5];
uint32_t FxdTiCtrl::y_Ni[5];
#endif
#if HAS_EXTRUDERS
// Linear advance variables.
float FxdTiCtrl::e_raw_z1 = 0.0f; // (ms) Unit delay of raw extruder position.
float FxdTiCtrl::e_advanced_z1 = 0.0f; // (ms) Unit delay of advanced extruder position.
#endif
//-----------------------------------------------------------------//
// Function definitions.
//-----------------------------------------------------------------//
// Public functions.
// Sets controller states to begin processing a block.
void FxdTiCtrl::startBlockProc(block_t * const current_block) {
current_block_cpy = current_block;
blockProcRdy = true;
blockProcDn = false;
runoutEna = true;
}
// Moves any free data points to the stepper buffer even if a full batch isn't ready.
void FxdTiCtrl::runoutBlock() {
if (runoutEna && !batchRdy) { // If the window is full already (block intervals was a multiple of
// the batch size), or runout is not enabled, no runout is needed.
// Fill out the trajectory window with the last position calculated.
if (makeVector_batchIdx > FTM_BATCH_SIZE) {
for (uint32_t i = makeVector_batchIdx; i < 2 * (FTM_BATCH_SIZE); i++) {
xd[i] = xd[makeVector_batchIdx - 1];
TERN_(HAS_Y_AXIS, yd[i] = yd[makeVector_batchIdx - 1]);
TERN_(HAS_Y_AXIS, zd[i] = zd[makeVector_batchIdx - 1]);
TERN_(HAS_EXTRUDERS, ed[i] = ed[makeVector_batchIdx - 1]);
}
}
makeVector_batchIdx = FTM_BATCH_SIZE;
batchRdy = true;
}
runoutEna = false;
}
// Controller main, to be invoked from non-isr task.
void FxdTiCtrl::loop() {
if (!cfg_mode) return;
static bool initd = false;
if (!initd) { init(); initd = true; }
// 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
reset();
blockProcDn = true; // Set queueing to look for next block.
runoutEna = false; // Disabling running out this block, since we want to halt the motion.
stepper.abort_current_block = false; // Abort finished.
}
// Planner processing and block conversion.
if (!blockProcRdy) stepper.fxdTiCtrl_BlockQueueUpdate();
if (blockProcRdy) {
if (!blockProcRdy_z1) loadBlockData(current_block_cpy); // One-shot.
while (!blockProcDn && !batchRdy && (makeVector_idx - makeVector_idx_z1 < (FTM_POINTS_PER_LOOP)))
makeVector();
}
// FBS / post processing.
if (batchRdy && !batchRdyForInterp) {
// Call Ulendo FBS here.
memcpy(xm, &xd[FTM_BATCH_SIZE], sizeof(xm));
TERN_(HAS_Y_AXIS, memcpy(ym, &yd[FTM_BATCH_SIZE], sizeof(ym)));
// Done compensating ...
// Copy the uncompensated vectors.
TERN_(HAS_Z_AXIS, memcpy(zm, &zd[FTM_BATCH_SIZE], sizeof(zm)));
TERN_(HAS_EXTRUDERS, memcpy(em, &ed[FTM_BATCH_SIZE], sizeof(em)));
// Shift the time series back in the window.
memcpy(xd, &xd[FTM_BATCH_SIZE], sizeof(xd) / 2);
TERN_(HAS_Y_AXIS, memcpy(yd, &yd[FTM_BATCH_SIZE], sizeof(yd) / 2));
// Disabled by comment as these are uncompensated, the lower half is not used.
//TERN_(HAS_Z_AXIS, memcpy(zd, &zd[FTM_BATCH_SIZE], (sizeof(zd) / 2)));
//TERN_(HAS_EXTRUDERS, memcpy(ed, &ed[FTM_BATCH_SIZE], (sizeof(ed) / 2)));
// ... data is ready in xm, ym, zm, em.
batchRdyForInterp = true;
batchRdy = false; // Clear so that makeVector() may resume generating points.
} // if (batchRdy && !batchRdyForInterp)
// Interpolation.
while ( batchRdyForInterp
&& ( stepperCmdBuffItems() < ((FTM_STEPPERCMD_BUFF_SIZE) - (FTM_STEPS_PER_UNIT_TIME)) )
&& ( (interpIdx - interpIdx_z1) < (FTM_STEPS_PER_LOOP) )
) {
convertToSteps(interpIdx);
if (++interpIdx == FTM_BATCH_SIZE) {
batchRdyForInterp = false;
interpIdx = 0;
}
}
// Report busy status to planner.
planner.fxdTiCtrl_busy = (sts_stepperBusy || ((!blockProcDn && blockProcRdy) || batchRdy || batchRdyForInterp || runoutEna));
blockProcRdy_z1 = blockProcRdy;
makeVector_idx_z1 = makeVector_idx;
interpIdx_z1 = interpIdx;
}
#if HAS_X_AXIS
// Refresh the gains used by shaping functions.
// To be called on init or mode or zeta change.
void FxdTiCtrl::updateShapingA(const_float_t zeta/*=FTM_SHAPING_ZETA*/, const_float_t vtol/*=FTM_SHAPING_V_TOL*/) {
const float K = exp( -zeta * PI / sqrt(1.0f - sq(zeta)) ),
K2 = sq(K);
switch (cfg_mode) {
case ftMotionMode_ZV:
xy_max_i = 1U;
x_Ai[0] = 1.0f / (1.0f + K);
x_Ai[1] = x_Ai[0] * K;
break;
case ftMotionMode_ZVD:
xy_max_i = 2U;
x_Ai[0] = 1.0f / ( 1.0f + 2.0f * K + K2 );
x_Ai[1] = x_Ai[0] * 2.0f * K;
x_Ai[2] = x_Ai[0] * K2;
break;
case ftMotionMode_EI: {
xy_max_i = 2U;
x_Ai[0] = 0.25f * (1.0f + vtol);
x_Ai[1] = 0.50f * (1.0f - vtol) * K;
x_Ai[2] = x_Ai[0] * K2;
const float A_adj = 1.0f / (x_Ai[0] + x_Ai[1] + x_Ai[2]);
for (uint32_t i = 0U; i < 3U; i++) { x_Ai[i] *= A_adj; }
} break;
case ftMotionMode_2HEI: {
xy_max_i = 3U;
const float vtol2 = sq(vtol);
const float X = pow(vtol2 * (sqrt(1.0f - vtol2) + 1.0f), 1.0f / 3.0f);
x_Ai[0] = ( 3.0f * sq(X) + 2.0f * X + 3.0f * vtol2 ) / (16.0f * X);
x_Ai[1] = ( 0.5f - x_Ai[0] ) * K;
x_Ai[2] = x_Ai[1] * K;
x_Ai[3] = x_Ai[0] * cu(K);
const float A_adj = 1.0f / (x_Ai[0] + x_Ai[1] + x_Ai[2] + x_Ai[3]);
for (uint32_t i = 0U; i < 4U; i++) { x_Ai[i] *= A_adj; }
} break;
case ftMotionMode_3HEI: {
xy_max_i = 4U;
x_Ai[0] = 0.0625f * ( 1.0f + 3.0f * vtol + 2.0f * sqrt( 2.0f * ( vtol + 1.0f ) * vtol ) );
x_Ai[1] = 0.25f * ( 1.0f - vtol ) * K;
x_Ai[2] = ( 0.5f * ( 1.0f + vtol ) - 2.0f * x_Ai[0] ) * K2;
x_Ai[3] = x_Ai[1] * K2;
x_Ai[4] = x_Ai[0] * sq(K2);
const float A_adj = 1.0f / (x_Ai[0] + x_Ai[1] + x_Ai[2] + x_Ai[3] + x_Ai[4]);
for (uint32_t i = 0U; i < 5U; i++) { x_Ai[i] *= A_adj; }
} break;
case ftMotionMode_MZV: {
xy_max_i = 2U;
const float B = 1.4142135623730950488016887242097f * K;
x_Ai[0] = 1.0f / (1.0f + B + K2);
x_Ai[1] = x_Ai[0] * B;
x_Ai[2] = x_Ai[0] * K2;
} break;
default:
for (uint32_t i = 0U; i < 5U; i++) x_Ai[i] = 0.0f;
xy_max_i = 0;
}
#if HAS_Y_AXIS
memcpy(y_Ai, x_Ai, sizeof(x_Ai)); // For now, zeta and vtol are shared across x and y.
#endif
}
// Refresh the indices used by shaping functions.
// To be called when frequencies change.
void FxdTiCtrl::updateShapingN(const_float_t xf OPTARG(HAS_Y_AXIS, const_float_t yf), const_float_t zeta/*=FTM_SHAPING_ZETA*/) {
// Protections omitted for DBZ and for index exceeding array length.
const float df = sqrt(1.0f - sq(zeta));
switch (cfg_mode) {
case ftMotionMode_ZV:
x_Ni[1] = round((0.5f / xf / df) * (FTM_FS));
#if HAS_Y_AXIS
y_Ni[1] = round((0.5f / yf / df) * (FTM_FS));
#endif
break;
case ftMotionMode_ZVD:
case ftMotionMode_EI:
x_Ni[1] = round((0.5f / xf / df) * (FTM_FS));
x_Ni[2] = 2 * x_Ni[1];
#if HAS_Y_AXIS
y_Ni[1] = round((0.5f / yf / df) * (FTM_FS));
y_Ni[2] = 2 * y_Ni[1];
#endif
break;
case ftMotionMode_2HEI:
x_Ni[1] = round((0.5f / xf / df) * (FTM_FS));
x_Ni[2] = 2 * x_Ni[1];
x_Ni[3] = 3 * x_Ni[1];
#if HAS_Y_AXIS
y_Ni[1] = round((0.5f / yf / df) * (FTM_FS));
y_Ni[2] = 2 * y_Ni[1];
y_Ni[3] = 3 * y_Ni[1];
#endif
break;
case ftMotionMode_3HEI:
x_Ni[1] = round((0.5f / xf / df) * (FTM_FS));
x_Ni[2] = 2 * x_Ni[1];
x_Ni[3] = 3 * x_Ni[1];
x_Ni[4] = 4 * x_Ni[1];
#if HAS_Y_AXIS
y_Ni[1] = round((0.5f / yf / df) * (FTM_FS));
y_Ni[2] = 2 * y_Ni[1];
y_Ni[3] = 3 * y_Ni[1];
y_Ni[4] = 4 * y_Ni[1];
#endif
break;
case ftMotionMode_MZV:
x_Ni[1] = round((0.375f / xf / df) * (FTM_FS));
x_Ni[2] = 2 * x_Ni[1];
#if HAS_Y_AXIS
y_Ni[1] = round((0.375f / yf / df) * (FTM_FS));
y_Ni[2] = 2 * y_Ni[1];
#endif
break;
default:
for (uint32_t i = 0U; i < 5U; i++) { x_Ni[i] = 0; TERN_(HAS_Y_AXIS, y_Ni[i] = 0); }
}
}
#endif // HAS_X_AXIS
// Reset all trajectory processing variables.
void FxdTiCtrl::reset() {
stepperCmdBuff_produceIdx = stepperCmdBuff_consumeIdx = 0;
for (uint32_t i = 0U; i < (FTM_BATCH_SIZE); i++) { // Reset trajectory history
TERN_(HAS_X_AXIS, xd[i] = 0.0f);
TERN_(HAS_Y_AXIS, yd[i] = 0.0f);
TERN_(HAS_Z_AXIS, zd[i] = 0.0f);
TERN_(HAS_EXTRUDERS, ed[i] = 0.0f);
}
blockProcRdy = blockProcRdy_z1 = blockProcDn = false;
batchRdy = batchRdyForInterp = false;
runoutEna = false;
TERN_(HAS_X_AXIS, x_endPosn_prevBlock = 0.0f);
TERN_(HAS_Y_AXIS, y_endPosn_prevBlock = 0.0f);
TERN_(HAS_Z_AXIS, z_endPosn_prevBlock = 0.0f);
TERN_(HAS_EXTRUDERS, e_endPosn_prevBlock = 0.0f);
makeVector_idx = makeVector_idx_z1 = 0;
makeVector_batchIdx = FTM_BATCH_SIZE;
TERN_(HAS_X_AXIS, x_steps = 0);
TERN_(HAS_Y_AXIS, y_steps = 0);
TERN_(HAS_Z_AXIS, z_steps = 0);
TERN_(HAS_EXTRUDERS, e_steps = 0);
interpIdx = interpIdx_z1 = 0;
TERN_(HAS_X_AXIS, x_dirState = stepDirState_NOT_SET);
TERN_(HAS_Y_AXIS, y_dirState = stepDirState_NOT_SET);
TERN_(HAS_Z_AXIS, z_dirState = stepDirState_NOT_SET);
TERN_(HAS_EXTRUDERS, e_dirState = stepDirState_NOT_SET);
nextStepTicks = FTM_MIN_TICKS;
#if HAS_X_AXIS
for (uint32_t i = 0U; i < (FTM_ZMAX); i++) { xd_zi[i] = 0.0f; TERN_(HAS_Y_AXIS, yd_zi[i] = 0.0f); }
xy_zi_idx = 0;
#endif
TERN_(HAS_EXTRUDERS, e_raw_z1 = e_advanced_z1 = 0.0f);
}
// Private functions.
// Auxiliary function to get number of step commands in the buffer.
uint32_t FxdTiCtrl::stepperCmdBuffItems() {
const uint32_t udiff = stepperCmdBuff_produceIdx - stepperCmdBuff_consumeIdx;
return stepperCmdBuff_produceIdx < stepperCmdBuff_consumeIdx ? (FTM_STEPPERCMD_BUFF_SIZE) + udiff : udiff;
}
// Initializes storage variables before startup.
void FxdTiCtrl::init() {
#if HAS_X_AXIS
updateShapingN(cfg_baseFreq[0] OPTARG(HAS_Y_AXIS, cfg_baseFreq[1]));
updateShapingA(FTM_SHAPING_ZETA, FTM_SHAPING_V_TOL);
#endif
reset(); // Precautionary.
}
// Loads / converts block data from planner to fixed-time control variables.
void FxdTiCtrl::loadBlockData(block_t * const current_block) {
const float totalLength = current_block->millimeters,
oneOverLength = 1.0f / totalLength;
const axis_bits_t direction = current_block->direction_bits;
#if HAS_X_AXIS
x_startPosn = x_endPosn_prevBlock;
float x_moveDist = current_block->steps.a / planner.settings.axis_steps_per_mm[X_AXIS];
if (TEST(direction, X_AXIS)) x_moveDist *= -1.0f;
x_Ratio = x_moveDist * oneOverLength;
#endif
#if HAS_Y_AXIS
y_startPosn = y_endPosn_prevBlock;
float y_moveDist = current_block->steps.b / planner.settings.axis_steps_per_mm[Y_AXIS];
if (TEST(direction, Y_AXIS)) y_moveDist *= -1.0f;
y_Ratio = y_moveDist * oneOverLength;
#endif
#if HAS_Z_AXIS
z_startPosn = z_endPosn_prevBlock;
float z_moveDist = current_block->steps.c / planner.settings.axis_steps_per_mm[Z_AXIS];
if (TEST(direction, Z_AXIS)) z_moveDist *= -1.0f;
z_Ratio = z_moveDist * oneOverLength;
#endif
#if HAS_EXTRUDERS
e_startPosn = e_endPosn_prevBlock;
float extrusion = current_block->steps.e / planner.settings.axis_steps_per_mm[E_AXIS_N(current_block->extruder)];
if (TEST(direction, E_AXIS_N(current_block->extruder))) extrusion *= -1.0f;
e_Ratio = extrusion * oneOverLength;
#endif
const float spm = totalLength / current_block->step_event_count; // (steps/mm) Distance for each step
f_s = spm * current_block->initial_rate; // (steps/s) Start feedrate
const float f_e = spm * current_block->final_rate; // (steps/s) End feedrate
const float a = current_block->acceleration, // (mm/s^2) Same magnitude for acceleration or deceleration
oneby2a = 1.0f / (2.0f * a), // (s/mm) Time to accelerate or decelerate one mm (i.e., oneby2a * 2
oneby2d = -oneby2a; // (s/mm) Time to accelerate or decelerate one mm (i.e., oneby2a * 2
const float fsSqByTwoA = sq(f_s) * oneby2a, // (mm) Distance to accelerate from start speed to nominal speed
feSqByTwoD = sq(f_e) * oneby2d; // (mm) Distance to decelerate from nominal speed to end speed
float F_n = current_block->nominal_speed; // (mm/s) Speed we hope to achieve, if possible
const float fdiff = feSqByTwoD - fsSqByTwoA, // (mm) Coasting distance if nominal speed is reached
odiff = oneby2a - oneby2d, // (i.e., oneby2a * 2) (mm/s) Change in speed for one second of acceleration
ldiff = totalLength - fdiff; // (mm) Distance to travel if nominal speed is reached
float T2 = (1.0f / F_n) * (ldiff - odiff * sq(F_n)); // (s) Coasting duration after nominal speed reached
if (T2 < 0.0f) {
T2 = 0.0f;
F_n = SQRT(ldiff / odiff); // Clip by intersection if nominal speed can't be reached.
}
const float T1 = (F_n - f_s) / a, // (s) Accel Time = difference in feedrate over acceleration
T3 = (F_n - f_e) / a; // (s) Decel Time = difference in feedrate over acceleration
N1 = ceil(T1 * (FTM_FS)); // Accel datapoints based on Hz frequency
N2 = ceil(T2 * (FTM_FS)); // Coast
N3 = ceil(T3 * (FTM_FS)); // Decel
const float T1_P = N1 * (FTM_TS), // (s) Accel datapoints x timestep resolution
T2_P = N2 * (FTM_TS), // (s) Coast
T3_P = N3 * (FTM_TS); // (s) Decel
// Calculate the reachable feedrate at the end of the accel phase
// totalLength is the total distance to travel in mm
// f_s is the starting feedrate in mm/s
// f_e is the ending feedrate in mm/s
// T1_P is the time spent accelerating in seconds
// T2_P is the time spent coasting in seconds
// T3_P is the time spent decelerating in seconds
// f_s * T1_P is the distance traveled during the accel phase
// f_e * T3_P is the distance traveled during the decel 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;
decel_P = (f_e - F_P) / T3_P;
// Calculate the distance traveled during the accel phase
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;
// One less than (Accel + Coasting + Decel) datapoints
max_intervals = N1 + N2 + N3 - 1U;
TERN_(HAS_X_AXIS, x_endPosn_prevBlock += x_moveDist);
TERN_(HAS_Y_AXIS, y_endPosn_prevBlock += y_moveDist);
TERN_(HAS_Z_AXIS, z_endPosn_prevBlock += z_moveDist);
TERN_(HAS_EXTRUDERS, e_endPosn_prevBlock += extrusion);
}
// Generate data points of the trajectory.
void FxdTiCtrl::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
accel_k = accel_P; // (mm/s^2) Acceleration K factor from Accel phase
}
else if (makeVector_idx >= N1 && makeVector_idx < (N1 + N2)) {
// Coasting phase
dist = s_1e + F_P * (tau - N1 * (FTM_TS)); // (mm) Distance traveled for coasting phase
//accel_k = 0.0f;
}
else {
// Deceleration phase
const float tau_ = 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
accel_k = decel_P; // (mm/s^2) Acceleration K factor from Decel phase
}
TERN_(HAS_X_AXIS, xd[makeVector_batchIdx] = x_startPosn + x_Ratio * dist); // (mm) X position for this datapoint
TERN_(HAS_Y_AXIS, yd[makeVector_batchIdx] = y_startPosn + y_Ratio * dist); // (mm) Y
TERN_(HAS_Z_AXIS, zd[makeVector_batchIdx] = z_startPosn + z_Ratio * dist); // (mm) Z
#if HAS_EXTRUDERS
const float new_raw_z1 = e_startPosn + e_Ratio * dist;
if (cfg_linearAdvEna) {
float dedt_adj = (new_raw_z1 - e_raw_z1) * (FTM_FS);
if (e_Ratio > 0.0f) dedt_adj += accel_k * cfg_linearAdvK;
e_advanced_z1 += dedt_adj * (FTM_TS);
ed[makeVector_batchIdx] = e_advanced_z1;
e_raw_z1 = new_raw_z1;
}
else {
ed[makeVector_batchIdx] = new_raw_z1;
// Alternatively: coordArray_e[makeVector_batchIdx] = e_startDist + extrusion / (N1 + N2 + N3);
}
#endif
// Update shaping parameters if needed.
#if HAS_Z_AXIS
static float zd_z1 = 0.0f;
#endif
switch (cfg_dynFreqMode) {
#if HAS_Z_AXIS
case dynFreqMode_Z_BASED:
if (zd[makeVector_batchIdx] != zd_z1) { // Only update if Z changed.
const float xf = cfg_baseFreq[0] + cfg_dynFreqK[0] * zd[makeVector_batchIdx],
yf = cfg_baseFreq[1] + cfg_dynFreqK[1] * zd[makeVector_batchIdx];
updateShapingN(_MAX(xf, FTM_MIN_SHAPE_FREQ), _MAX(yf, FTM_MIN_SHAPE_FREQ));
zd_z1 = zd[makeVector_batchIdx];
}
break;
#endif
#if HAS_X_AXIS && HAS_EXTRUDERS
case dynFreqMode_MASS_BASED:
// Update constantly. The optimization done for Z value makes
// less sense for E, as E is expected to constantly change.
updateShapingN( cfg_baseFreq[0] + cfg_dynFreqK[0] * ed[makeVector_batchIdx]
OPTARG(HAS_Y_AXIS, cfg_baseFreq[1] + cfg_dynFreqK[1] * ed[makeVector_batchIdx]) );
break;
#endif
default: break;
}
// Apply shaping if in mode.
#if HAS_X_AXIS
if (WITHIN(cfg_mode, 10U, 19U)) {
xd_zi[xy_zi_idx] = xd[makeVector_batchIdx];
xd[makeVector_batchIdx] *= x_Ai[0];
#if HAS_Y_AXIS
yd_zi[xy_zi_idx] = yd[makeVector_batchIdx];
yd[makeVector_batchIdx] *= y_Ai[0];
#endif
for (uint32_t i = 1U; i <= xy_max_i; i++) {
const uint32_t udiffx = xy_zi_idx - x_Ni[i];
xd[makeVector_batchIdx] += x_Ai[i] * xd_zi[x_Ni[i] > xy_zi_idx ? (FTM_ZMAX) + udiffx : udiffx];
#if HAS_Y_AXIS
const uint32_t udiffy = xy_zi_idx - y_Ni[i];
yd[makeVector_batchIdx] += y_Ai[i] * yd_zi[y_Ni[i] > xy_zi_idx ? (FTM_ZMAX) + udiffy : udiffy];
#endif
}
if (++xy_zi_idx == (FTM_ZMAX)) xy_zi_idx = 0;
}
#endif
// Filled up the queue with regular and shaped steps
if (++makeVector_batchIdx == 2 * (FTM_BATCH_SIZE)) {
makeVector_batchIdx = FTM_BATCH_SIZE;
batchRdy = true;
}
if (makeVector_idx == max_intervals) {
blockProcDn = true;
blockProcRdy = false;
makeVector_idx = 0;
}
else
makeVector_idx++;
}
// Interpolates single data point to stepper commands.
void FxdTiCtrl::convertToSteps(const uint32_t idx) {
#if HAS_X_AXIS
int32_t x_err_P = 0;
#endif
#if HAS_Y_AXIS
int32_t y_err_P = 0;
#endif
#if HAS_Z_AXIS
int32_t z_err_P = 0;
#endif
#if HAS_EXTRUDERS
int32_t e_err_P = 0;
#endif
//#define STEPS_ROUNDING
#if ENABLED(STEPS_ROUNDING)
#if HAS_X_AXIS
const float x_steps_tar = xm[idx] * planner.settings.axis_steps_per_mm[X_AXIS] + (xm[idx] < 0.0f ? -0.5f : 0.5f); // May be eliminated if guaranteed positive.
const int32_t x_delta = int32_t(x_steps_tar) - x_steps;
#endif
#if HAS_Y_AXIS
const float y_steps_tar = ym[idx] * planner.settings.axis_steps_per_mm[Y_AXIS] + (ym[idx] < 0.0f ? -0.5f : 0.5f);
const int32_t y_delta = int32_t(y_steps_tar) - y_steps;
#endif
#if HAS_Z_AXIS
const float z_steps_tar = zm[idx] * planner.settings.axis_steps_per_mm[Z_AXIS] + (zm[idx] < 0.0f ? -0.5f : 0.5f);
const int32_t z_delta = int32_t(z_steps_tar) - z_steps;
#endif
#if HAS_EXTRUDERS
const float e_steps_tar = em[idx] * planner.settings.axis_steps_per_mm[E_AXIS] + (em[idx] < 0.0f ? -0.5f : 0.5f);
const int32_t e_delta = int32_t(e_steps_tar) - e_steps;
#endif
#else
#if HAS_X_AXIS
const int32_t x_delta = int32_t(xm[idx] * planner.settings.axis_steps_per_mm[X_AXIS]) - x_steps;
#endif
#if HAS_Y_AXIS
const int32_t y_delta = int32_t(ym[idx] * planner.settings.axis_steps_per_mm[Y_AXIS]) - y_steps;
#endif
#if HAS_Z_AXIS
const int32_t z_delta = int32_t(zm[idx] * planner.settings.axis_steps_per_mm[Z_AXIS]) - z_steps;
#endif
#if HAS_EXTRUDERS
const int32_t e_delta = int32_t(em[idx] * planner.settings.axis_steps_per_mm[E_AXIS]) - e_steps;
#endif
#endif
bool any_dirChange = (false
|| TERN0(HAS_X_AXIS, (x_delta > 0 && x_dirState != stepDirState_POS) || (x_delta < 0 && x_dirState != stepDirState_NEG))
|| TERN0(HAS_Y_AXIS, (y_delta > 0 && y_dirState != stepDirState_POS) || (y_delta < 0 && y_dirState != stepDirState_NEG))
|| TERN0(HAS_Z_AXIS, (z_delta > 0 && z_dirState != stepDirState_POS) || (z_delta < 0 && z_dirState != stepDirState_NEG))
|| TERN0(HAS_EXTRUDERS, (e_delta > 0 && e_dirState != stepDirState_POS) || (e_delta < 0 && e_dirState != stepDirState_NEG))
);
for (uint32_t i = 0U; i < (FTM_STEPS_PER_UNIT_TIME); i++) {
// TODO: (?) Since the *delta variables will not change,
// the comparison may be done once before iterating at
// expense of storage and lines of code.
bool anyStep = false;
stepperCmdBuff[stepperCmdBuff_produceIdx] = 0;
// Commands are written in the format:
// |X_step|X_direction|Y_step|Y_direction|Z_step|Z_direction|E_step|E_direction|
#if HAS_X_AXIS
if (x_delta >= 0) {
if ((x_err_P + x_delta) < (FTM_CTS_COMPARE_VAL)) {
x_err_P += x_delta;
}
else {
x_steps++;
stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_X) | _BV(FT_BIT_STEP_X);
x_err_P += x_delta - (FTM_STEPS_PER_UNIT_TIME);
anyStep = true;
}
}
else {
if ((x_err_P + x_delta) > -(FTM_CTS_COMPARE_VAL)) {
x_err_P += x_delta;
}
else {
x_steps--;
stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_STEP_X);
x_err_P += x_delta + (FTM_STEPS_PER_UNIT_TIME);
anyStep = true;
}
}
#endif // HAS_X_AXIS
#if HAS_Y_AXIS
if (y_delta >= 0) {
if ((y_err_P + y_delta) < (FTM_CTS_COMPARE_VAL)) {
y_err_P += y_delta;
}
else {
y_steps++;
stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_Y) | _BV(FT_BIT_STEP_Y);
y_err_P += y_delta - (FTM_STEPS_PER_UNIT_TIME);
anyStep = true;
}
}
else {
if ((y_err_P + y_delta) > -(FTM_CTS_COMPARE_VAL)) {
y_err_P += y_delta;
}
else {
y_steps--;
stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_STEP_Y);
y_err_P += y_delta + (FTM_STEPS_PER_UNIT_TIME);
anyStep = true;
}
}
#endif // HAS_Y_AXIS
#if HAS_Z_AXIS
if (z_delta >= 0) {
if ((z_err_P + z_delta) < (FTM_CTS_COMPARE_VAL)) {
z_err_P += z_delta;
}
else {
z_steps++;
stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_Z) | _BV(FT_BIT_STEP_Z);
z_err_P += z_delta - (FTM_STEPS_PER_UNIT_TIME);
anyStep = true;
}
}
else {
if ((z_err_P + z_delta) > -(FTM_CTS_COMPARE_VAL)) {
z_err_P += z_delta;
}
else {
z_steps--;
stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_STEP_Z);
z_err_P += z_delta + (FTM_STEPS_PER_UNIT_TIME);
anyStep = true;
}
}
#endif // HAS_Z_AXIS
#if HAS_EXTRUDERS
if (e_delta >= 0) {
if ((e_err_P + e_delta) < (FTM_CTS_COMPARE_VAL)) {
e_err_P += e_delta;
}
else {
e_steps++;
stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_E) | _BV(FT_BIT_STEP_E);
e_err_P += e_delta - (FTM_STEPS_PER_UNIT_TIME);
anyStep = true;
}
}
else {
if ((e_err_P + e_delta) > -(FTM_CTS_COMPARE_VAL)) {
e_err_P += e_delta;
}
else {
e_steps--;
stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_STEP_E);
e_err_P += e_delta + (FTM_STEPS_PER_UNIT_TIME);
anyStep = true;
}
}
#endif // HAS_EXTRUDERS
if (!anyStep) {
nextStepTicks += (FTM_MIN_TICKS);
}
else {
stepperCmdBuff_StepRelativeTi[stepperCmdBuff_produceIdx] = nextStepTicks;
const uint8_t dir_index = stepperCmdBuff_produceIdx >> 3,
dir_bit = stepperCmdBuff_produceIdx & 0x7;
if (any_dirChange) {
SBI(stepperCmdBuff_ApplyDir[dir_index], dir_bit);
#if HAS_X_AXIS
if (x_delta > 0) {
stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_X);
x_dirState = stepDirState_POS;
}
else {
x_dirState = stepDirState_NEG;
}
#endif
#if HAS_Y_AXIS
if (y_delta > 0) {
stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_Y);
y_dirState = stepDirState_POS;
}
else {
y_dirState = stepDirState_NEG;
}
#endif
#if HAS_Z_AXIS
if (z_delta > 0) {
stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_Z);
z_dirState = stepDirState_POS;
}
else {
z_dirState = stepDirState_NEG;
}
#endif
#if HAS_EXTRUDERS
if (e_delta > 0) {
stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_E);
e_dirState = stepDirState_POS;
}
else {
e_dirState = stepDirState_NEG;
}
#endif
any_dirChange = false;
}
else { // ...no direction change.
CBI(stepperCmdBuff_ApplyDir[dir_index], dir_bit);
}
if (stepperCmdBuff_produceIdx == (FTM_STEPPERCMD_BUFF_SIZE) - 1) {
stepperCmdBuff_produceIdx = 0;
}
else {
stepperCmdBuff_produceIdx++;
}
nextStepTicks = FTM_MIN_TICKS;
}
} // FTM_STEPS_PER_UNIT_TIME loop
}
#endif // FT_MOTION

170
Marlin/src/module/ft_motion.h Normal file
View file

@ -0,0 +1,170 @@
/**
* 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 <https://www.gnu.org/licenses/>.
*
*/
#pragma once
#include "../inc/MarlinConfigPre.h" // Access the top level configurations.
#include "../module/planner.h" // Access block type from planner.
#include "ft_types.h"
#define FTM_STEPPERCMD_DIR_SIZE ((FTM_STEPPERCMD_BUFF_SIZE + 7) / 8)
class FxdTiCtrl {
public:
// Public variables
static ftMotionMode_t cfg_mode; // Mode / active compensation mode configuration.
static bool cfg_linearAdvEna; // Linear advance enable configuration.
static float cfg_linearAdvK; // Linear advance gain.
static dynFreqMode_t cfg_dynFreqMode; // Dynamic frequency mode configuration.
#if HAS_X_AXIS
static float cfg_baseFreq[1 + ENABLED(HAS_Y_AXIS)]; // Base frequency. [Hz]
static float cfg_dynFreqK[1 + ENABLED(HAS_Y_AXIS)]; // Scaling / gain for dynamic frequency. [Hz/mm] or [Hz/g]
#endif
static uint8_t stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE]; // Buffer of stepper commands.
static hal_timer_t stepperCmdBuff_StepRelativeTi[FTM_STEPPERCMD_BUFF_SIZE]; // Buffer of the stepper command timing.
static uint8_t stepperCmdBuff_ApplyDir[FTM_STEPPERCMD_DIR_SIZE]; // Buffer of whether DIR needs to be updated.
static uint32_t stepperCmdBuff_produceIdx, // Index of next stepper command write to the buffer.
stepperCmdBuff_consumeIdx; // Index of next stepper command read from the buffer.
static bool sts_stepperBusy; // The stepper buffer has items and is in use.
// Public methods
static void startBlockProc(block_t * const current_block); // Set controller states to begin processing a block.
static bool getBlockProcDn() { return blockProcDn; } // Return true if the controller no longer needs the current block.
static void runoutBlock(); // Move any free data points to the stepper buffer even if a full batch isn't ready.
static void loop(); // Controller main, to be invoked from non-isr task.
#if HAS_X_AXIS
// Refresh the gains used by shaping functions.
// To be called on init or mode or zeta change.
static void updateShapingA(const_float_t zeta=FTM_SHAPING_ZETA, const_float_t vtol=FTM_SHAPING_V_TOL);
// Refresh the indices used by shaping functions.
// To be called when frequencies change.
static void updateShapingN(const_float_t xf OPTARG(HAS_Y_AXIS, const_float_t yf), const_float_t zeta=FTM_SHAPING_ZETA);
#endif
static void reset(); // Resets all states of the fixed time conversion to defaults.
private:
#if HAS_X_AXIS
static float xd[2 * (FTM_BATCH_SIZE)], xm[FTM_BATCH_SIZE];
#endif
#if HAS_Y_AXIS
static float yd[2 * (FTM_BATCH_SIZE)], ym[FTM_BATCH_SIZE];
#endif
#if HAS_Z_AXIS
static float zd[2 * (FTM_BATCH_SIZE)], zm[FTM_BATCH_SIZE];
#endif
#if HAS_EXTRUDERS
static float ed[2 * (FTM_BATCH_SIZE)], em[FTM_BATCH_SIZE];
#endif
static block_t *current_block_cpy;
static bool blockProcRdy, blockProcRdy_z1, blockProcDn;
static bool batchRdy, batchRdyForInterp;
static bool runoutEna;
// Trapezoid data variables.
#if HAS_X_AXIS
static float x_startPosn, x_endPosn_prevBlock, x_Ratio;
#endif
#if HAS_Y_AXIS
static float y_startPosn, y_endPosn_prevBlock, y_Ratio;
#endif
#if HAS_Z_AXIS
static float z_startPosn, z_endPosn_prevBlock, z_Ratio;
#endif
#if HAS_EXTRUDERS
static float e_startPosn, e_endPosn_prevBlock, e_Ratio;
#endif
static float accel_P, decel_P,
F_P,
f_s,
s_1e,
s_2e;
static uint32_t N1, N2, N3;
static uint32_t max_intervals;
// Make vector variables.
static uint32_t makeVector_idx,
makeVector_idx_z1,
makeVector_batchIdx;
// Interpolation variables.
static uint32_t interpIdx,
interpIdx_z1;
#if HAS_X_AXIS
static int32_t x_steps;
static stepDirState_t x_dirState;
#endif
#if HAS_Y_AXIS
static int32_t y_steps;
static stepDirState_t y_dirState;
#endif
#if HAS_Z_AXIS
static int32_t z_steps;
static stepDirState_t z_dirState;
#endif
#if HAS_EXTRUDERS
static int32_t e_steps;
static stepDirState_t e_dirState;
#endif
static hal_timer_t nextStepTicks;
// Shaping variables.
#if HAS_X_AXIS
static uint32_t xy_zi_idx, xy_max_i;
static float xd_zi[FTM_ZMAX];
static float x_Ai[5];
static uint32_t x_Ni[5];
#endif
#if HAS_Y_AXIS
static float yd_zi[FTM_ZMAX];
static float y_Ai[5];
static uint32_t y_Ni[5];
#endif
// Linear advance variables.
#if HAS_EXTRUDERS
static float e_raw_z1, e_advanced_z1;
#endif
// Private methods
static uint32_t stepperCmdBuffItems();
static void init();
static void loadBlockData(block_t * const current_block);
static void makeVector();
static void convertToSteps(const uint32_t idx);
}; // class fxdTiCtrl
extern FxdTiCtrl fxdTiCtrl;

59
Marlin/src/module/ft_types.h Normal file
View file

@ -0,0 +1,59 @@
/**
* 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 <https://www.gnu.org/licenses/>.
*
*/
#pragma once
#include "../core/types.h"
typedef enum FXDTICtrlMode : uint8_t {
ftMotionMode_DISABLED = 0U,
ftMotionMode_ENABLED = 1U,
ftMotionMode_ULENDO_FBS = 2U,
ftMotionMode_ZV = 10U,
ftMotionMode_ZVD = 11U,
ftMotionMode_EI = 12U,
ftMotionMode_2HEI = 13U,
ftMotionMode_3HEI = 14U,
ftMotionMode_MZV = 15U,
ftMotionMode_DISCTF = 20U
} ftMotionMode_t;
enum dynFreqMode_t : uint8_t {
dynFreqMode_DISABLED = 0U,
dynFreqMode_Z_BASED = 1U,
dynFreqMode_MASS_BASED = 2U
};
enum stepDirState_t {
stepDirState_NOT_SET = 0U,
stepDirState_POS = 1U,
stepDirState_NEG = 2U
};
enum {
FT_BIT_DIR_E, FT_BIT_STEP_E,
FT_BIT_DIR_Z, FT_BIT_STEP_Z,
FT_BIT_DIR_Y, FT_BIT_STEP_Y,
FT_BIT_DIR_X, FT_BIT_STEP_X,
FT_BIT_COUNT
};
typedef bits_t(FT_BIT_COUNT) ft_command_t;

22
Marlin/src/module/planner.cpp
View file

@ -69,6 +69,9 @@
#include "stepper.h"
#include "motion.h"
#include "temperature.h"
#if ENABLED(FT_MOTION)
#include "ft_motion.h"
#endif
#include "../lcd/marlinui.h"
#include "../gcode/parser.h"
@ -112,7 +115,8 @@
// Delay for delivery of first block to the stepper ISR, if the queue contains 2 or
// fewer movements. The delay is measured in milliseconds, and must be less than 250ms
#define BLOCK_DELAY_FOR_1ST_MOVE 100
#define BLOCK_DELAY_NONE 0U
#define BLOCK_DELAY_FOR_1ST_MOVE 100U
Planner planner;
@ -127,7 +131,7 @@ volatile uint8_t Planner::block_buffer_head, // Index of the next block to be
Planner::block_buffer_planned, // Index of the optimally planned block
Planner::block_buffer_tail; // Index of the busy block, if any
uint16_t Planner::cleaning_buffer_counter; // A counter to disable queuing of blocks
uint8_t Planner::delay_before_delivering; // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
uint8_t Planner::delay_before_delivering; // Delay block delivery so initial blocks in an empty queue may merge
planner_settings_t Planner::settings; // Initialized by settings.load()
@ -225,6 +229,10 @@ float Planner::previous_nominal_speed;
int32_t Planner::xy_freq_min_interval_us = LROUND(1000000.0f / (XY_FREQUENCY_LIMIT));
#endif
#if ENABLED(FT_MOTION)
bool Planner::fxdTiCtrl_busy = false;
#endif
#if ENABLED(LIN_ADVANCE)
float Planner::extruder_advance_K[DISTINCT_E]; // Initialized by settings.load()
#endif
@ -1683,7 +1691,8 @@ 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 = BLOCK_DELAY_FOR_1ST_MOVE;
delay_before_delivering = TERN_(FT_MOTION, fxdTiCtrl.cfg_mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE;
TERN_(HAS_WIRED_LCD, clear_block_buffer_runtime()); // Clear the accumulated runtime
@ -1729,6 +1738,7 @@ bool Planner::busy() {
return (has_blocks_queued() || cleaning_buffer_counter
|| TERN0(EXTERNAL_CLOSED_LOOP_CONTROLLER, CLOSED_LOOP_WAITING())
|| TERN0(HAS_ZV_SHAPING, stepper.input_shaping_busy())
|| TERN0(FT_MOTION, fxdTiCtrl_busy)
);
}
@ -1841,7 +1851,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 = BLOCK_DELAY_FOR_1ST_MOVE;
delay_before_delivering = TERN_(FT_MOTION, fxdTiCtrl.cfg_mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE;
}
// Move buffer head
@ -2945,7 +2955,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 = BLOCK_DELAY_FOR_1ST_MOVE;
delay_before_delivering = TERN_(FT_MOTION, fxdTiCtrl.cfg_mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE;
}
block_buffer_head = next_buffer_head;
@ -3243,7 +3253,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 = BLOCK_DELAY_FOR_1ST_MOVE;
delay_before_delivering = TERN_(FT_MOTION, fxdTiCtrl.cfg_mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE;
}
// Move buffer head

4
Marlin/src/module/planner.h
View file

@ -512,6 +512,10 @@ class Planner {
}
#endif
#if ENABLED(FT_MOTION)
static bool fxdTiCtrl_busy;
#endif
private:
/**

275
Marlin/src/module/stepper.cpp
View file

@ -91,6 +91,10 @@ Stepper stepper; // Singleton
#include "planner.h"
#include "motion.h"
#if ENABLED(FT_MOTION)
#include "ft_motion.h"
#endif
#include "../lcd/marlinui.h"
#include "../gcode/queue.h"
#include "../sd/cardreader.h"
@ -1488,63 +1492,133 @@ void Stepper::isr() {
// Limit the amount of iterations
uint8_t max_loops = 10;
#if ENABLED(FT_MOTION)
static bool fxdTiCtrl_stepCmdRdy = false; // Indicates a step command was loaded from the
// buffers and is ready to be output.
static bool fxdTiCtrl_applyDir = false; // Indicates the DIR output should be set.
static ft_command_t fxdTiCtrl_stepCmd = 0U; // Storage for the step command to be output.
static uint32_t fxdTiCtrl_nextAuxISR = 0U; // Storage for the next ISR of the auxilliary tasks.
#endif
// We need this variable here to be able to use it in the following loop
hal_timer_t min_ticks;
do {
// Enable ISRs to reduce USART processing latency
hal.isr_on();
TERN_(HAS_ZV_SHAPING, shaping_isr()); // Do Shaper stepping, if needed
hal_timer_t interval;
if (!nextMainISR) pulse_phase_isr(); // 0 = Do coordinated axes Stepper pulses
#if ENABLED(FT_MOTION)
#if ENABLED(LIN_ADVANCE)
if (!nextAdvanceISR) { // 0 = Do Linear Advance E Stepper pulses
advance_isr();
nextAdvanceISR = la_interval;
// NOTE STEPPER_TIMER_RATE is equal to 2000000, not what VSCode shows
const bool using_fxtictrl = fxdTiCtrl.cfg_mode;
if (using_fxtictrl) {
if (!nextMainISR) {
if (abort_current_block) {
fxdTiCtrl_stepCmdRdy = false; // If a command was ready, cancel it.
fxdTiCtrl.sts_stepperBusy = false; // Set busy false to allow a reset.
nextMainISR = 0.01f * (STEPPER_TIMER_RATE); // Come back in 10 msec.
}
else { // !(abort_current_block)
if (fxdTiCtrl_stepCmdRdy) {
fxdTiCtrl_stepper(fxdTiCtrl_applyDir, fxdTiCtrl_stepCmd);
fxdTiCtrl_stepCmdRdy = false;
}
// Check if there is data in the buffers.
if (fxdTiCtrl.stepperCmdBuff_produceIdx != fxdTiCtrl.stepperCmdBuff_consumeIdx) {
fxdTiCtrl.sts_stepperBusy = true;
// "Pop" one command from the command buffer.
fxdTiCtrl_stepCmd = fxdTiCtrl.stepperCmdBuff[fxdTiCtrl.stepperCmdBuff_consumeIdx];
const uint8_t dir_index = fxdTiCtrl.stepperCmdBuff_consumeIdx >> 3,
dir_bit = fxdTiCtrl.stepperCmdBuff_consumeIdx & 0x7;
fxdTiCtrl_applyDir = TEST(fxdTiCtrl.stepperCmdBuff_ApplyDir[dir_index], dir_bit);
nextMainISR = fxdTiCtrl.stepperCmdBuff_StepRelativeTi[fxdTiCtrl.stepperCmdBuff_consumeIdx];
fxdTiCtrl_stepCmdRdy = true;
if (++fxdTiCtrl.stepperCmdBuff_consumeIdx == (FTM_STEPPERCMD_BUFF_SIZE))
fxdTiCtrl.stepperCmdBuff_consumeIdx = 0;
}
else { // Buffer empty.
fxdTiCtrl.sts_stepperBusy = false;
nextMainISR = 0.01f * (STEPPER_TIMER_RATE); // Come back in 10 msec.
}
} // !(abort_current_block)
} // if (!nextMainISR)
// Define 2.5 msec task for auxilliary functions.
if (!fxdTiCtrl_nextAuxISR) {
endstops.update();
TERN_(INTEGRATED_BABYSTEPPING, if (babystep.has_steps()) babystepping_isr());
fxdTiCtrl_refreshAxisDidMove();
fxdTiCtrl_nextAuxISR = 0.0025f * (STEPPER_TIMER_RATE);
}
interval = _MIN(nextMainISR, fxdTiCtrl_nextAuxISR);
nextMainISR -= interval;
fxdTiCtrl_nextAuxISR -= interval;
}
else if (nextAdvanceISR == LA_ADV_NEVER) // Start LA steps if necessary
nextAdvanceISR = la_interval;
#else
constexpr bool using_fxtictrl = false;
#endif
#if ENABLED(INTEGRATED_BABYSTEPPING)
const bool is_babystep = (nextBabystepISR == 0); // 0 = Do Babystepping (XY)Z pulses
if (is_babystep) nextBabystepISR = babystepping_isr();
#endif
if (!using_fxtictrl) {
// ^== Time critical. NOTHING besides pulse generation should be above here!!!
TERN_(HAS_ZV_SHAPING, shaping_isr()); // Do Shaper stepping, if needed
if (!nextMainISR) nextMainISR = block_phase_isr(); // Manage acc/deceleration, get next block
if (!nextMainISR) pulse_phase_isr(); // 0 = Do coordinated axes Stepper pulses
#if ENABLED(INTEGRATED_BABYSTEPPING)
if (is_babystep) // Avoid ANY stepping too soon after baby-stepping
NOLESS(nextMainISR, (BABYSTEP_TICKS) / 8); // FULL STOP for 125µs after a baby-step
#if ENABLED(LIN_ADVANCE)
if (!nextAdvanceISR) { // 0 = Do Linear Advance E Stepper pulses
advance_isr();
nextAdvanceISR = la_interval;
}
else if (nextAdvanceISR == LA_ADV_NEVER) // Start LA steps if necessary
nextAdvanceISR = la_interval;
#endif
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
#if ENABLED(INTEGRATED_BABYSTEPPING)
const bool is_babystep = (nextBabystepISR == 0); // 0 = Do Babystepping (XY)Z pulses
if (is_babystep) nextBabystepISR = babystepping_isr();
#endif
// Get the interval to the next ISR call
const hal_timer_t interval = _MIN(
hal_timer_t(HAL_TIMER_TYPE_MAX), // Come back in a very long time
nextMainISR // Time until the next Pulse / Block phase
OPTARG(INPUT_SHAPING_X, ShapingQueue::peek_x()) // Time until next input shaping echo for X
OPTARG(INPUT_SHAPING_Y, ShapingQueue::peek_y()) // Time until next input shaping echo for Y
OPTARG(LIN_ADVANCE, nextAdvanceISR) // Come back early for Linear Advance?
OPTARG(INTEGRATED_BABYSTEPPING, nextBabystepISR) // Come back early for Babystepping?
);
// ^== Time critical. NOTHING besides pulse generation should be above here!!!
//
// Compute remaining time for each ISR phase
// NEVER : The phase is idle
// Zero : The phase will occur on the next ISR call
// Non-zero : The phase will occur on a future ISR call
//
if (!nextMainISR) nextMainISR = block_phase_isr(); // Manage acc/deceleration, get next block
nextMainISR -= interval;
TERN_(HAS_ZV_SHAPING, ShapingQueue::decrement_delays(interval));
TERN_(LIN_ADVANCE, if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval);
TERN_(INTEGRATED_BABYSTEPPING, if (nextBabystepISR != BABYSTEP_NEVER) nextBabystepISR -= interval);
#if ENABLED(INTEGRATED_BABYSTEPPING)
if (is_babystep) // Avoid ANY stepping too soon after baby-stepping
NOLESS(nextMainISR, (BABYSTEP_TICKS) / 8); // FULL STOP for 125µs after a baby-step
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
// Get the interval to the next ISR call
interval = _MIN(nextMainISR, uint32_t(HAL_TIMER_TYPE_MAX)); // Time until the next Pulse / Block phase
TERN_(INPUT_SHAPING_X, NOMORE(interval, ShapingQueue::peek_x())); // Time until next input shaping echo for X
TERN_(INPUT_SHAPING_Y, NOMORE(interval, ShapingQueue::peek_y())); // Time until next input shaping echo for Y
TERN_(LIN_ADVANCE, NOMORE(interval, nextAdvanceISR)); // Come back early for Linear Advance?
TERN_(INTEGRATED_BABYSTEPPING, NOMORE(interval, nextBabystepISR)); // Come back early for Babystepping?
//
// Compute remaining time for each ISR phase
// NEVER : The phase is idle
// Zero : The phase will occur on the next ISR call
// Non-zero : The phase will occur on a future ISR call
//
nextMainISR -= interval;
TERN_(HAS_ZV_SHAPING, ShapingQueue::decrement_delays(interval));
TERN_(LIN_ADVANCE, if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval);
TERN_(INTEGRATED_BABYSTEPPING, if (nextBabystepISR != BABYSTEP_NEVER) nextBabystepISR -= interval);
} // standard motion control
/**
* This needs to avoid a race-condition caused by interleaving
@ -1978,7 +2052,7 @@ void Stepper::pulse_phase_isr() {
#if ENABLED(MIXING_EXTRUDER)
if (step_needed.e) {
count_position[E_AXIS] += count_direction[E_AXIS];
count_position.e += count_direction.e;
E_STEP_WRITE(mixer.get_next_stepper(), STEP_STATE_E);
}
#elif HAS_E0_STEP
@ -3381,6 +3455,127 @@ void Stepper::report_positions() {
report_a_position(pos);
}
#if ENABLED(FT_MOTION)
// Set stepper I/O for fixed time controller.
void Stepper::fxdTiCtrl_stepper(const bool applyDir, const ft_command_t command) {
USING_TIMED_PULSE();
#if HAS_Z_AXIS
// Z is handled differently to update the stepper
// counts (needed by Marlin for bed level probing).
const bool z_dir = !TEST(command, FT_BIT_DIR_Z),
z_step = TEST(command, FT_BIT_STEP_Z);
#endif
if (applyDir) {
X_DIR_WRITE(TEST(command, FT_BIT_DIR_X));
TERN_(HAS_Y_AXIS, Y_DIR_WRITE(TEST(command, FT_BIT_DIR_Y)));
TERN_(HAS_Z_AXIS, Z_DIR_WRITE(z_dir));
TERN_(HAS_EXTRUDERS, E0_DIR_WRITE(TEST(command, FT_BIT_DIR_E)));
DIR_WAIT_AFTER();
}
X_STEP_WRITE(TEST(command, FT_BIT_STEP_X));
TERN_(HAS_Y_AXIS, Y_STEP_WRITE(TEST(command, FT_BIT_STEP_Y)));
TERN_(HAS_Z_AXIS, Z_STEP_WRITE(z_step));
TERN_(HAS_EXTRUDERS, E0_STEP_WRITE(TEST(command, FT_BIT_STEP_E)));
START_TIMED_PULSE();
#if HAS_Z_AXIS
// Update step counts
if (z_step) count_position.z += z_dir ? -1 : 1;
#endif
AWAIT_HIGH_PULSE();
X_STEP_WRITE(0);
TERN_(HAS_Y_AXIS, Y_STEP_WRITE(0));
TERN_(HAS_Z_AXIS, Z_STEP_WRITE(0));
TERN_(HAS_EXTRUDERS, E0_STEP_WRITE(0));
} // Stepper::fxdTiCtrl_stepper
void Stepper::fxdTiCtrl_BlockQueueUpdate() {
if (current_block) {
// If the current block is not done processing, return right away
if (!fxdTiCtrl.getBlockProcDn()) return;
axis_did_move = 0;
current_block = nullptr;
discard_current_block();
}
if (!current_block) { // No current block
// Check the buffer for a new block
current_block = planner.get_current_block();
if (current_block) {
// Sync block? Sync the stepper counts and return
while (current_block->is_sync()) {
if (!(current_block->is_fan_sync() || current_block->is_pwr_sync())) _set_position(current_block->position);
discard_current_block();
// Try to get a new block
if (!(current_block = planner.get_current_block()))
return; // No more queued movements!image.png
}
// this is needed by motor_direction() and subsequently bed leveling (somehow)
// update it here, even though it will may be out of sync with step commands
last_direction_bits = current_block->direction_bits;
fxdTiCtrl.startBlockProc(current_block);
}
else {
fxdTiCtrl.runoutBlock();
return; // No queued blocks
}
} // if (!current_block)
} // Stepper::fxdTiCtrl_BlockQueueUpdate()
// Debounces the axis move indication to account for potential
// delay between the block information and the stepper commands
void Stepper::fxdTiCtrl_refreshAxisDidMove() {
// Set the debounce time in seconds.
#define AXIS_DID_MOVE_DEB 5 // TODO: The debounce time should be calculated if possible,
// or the set conditions should be changed from the block to
// the motion trajectory or motor commands.
uint8_t axis_bits = 0U;
static uint32_t a_debounce = 0U;
if (!!current_block->steps.a) a_debounce = (AXIS_DID_MOVE_DEB) * 400; // divide by 0.0025f
if (a_debounce) { SBI(axis_bits, A_AXIS); a_debounce--; }
#if HAS_Y_AXIS
static uint32_t b_debounce = 0U;
if (!!current_block->steps.b) b_debounce = (AXIS_DID_MOVE_DEB) * 400;
if (b_debounce) { SBI(axis_bits, B_AXIS); b_debounce--; }
#endif
#if HAS_Z_AXIS
static uint32_t c_debounce = 0U;
if (!!current_block->steps.c) c_debounce = (AXIS_DID_MOVE_DEB) * 400;
if (c_debounce) { SBI(axis_bits, C_AXIS); c_debounce--; }
#endif
#if HAS_EXTRUDERS
static uint32_t e_debounce = 0U;
if (!!current_block->steps.e) e_debounce = (AXIS_DID_MOVE_DEB) * 400;
if (e_debounce) { SBI(axis_bits, E_AXIS); e_debounce--; }
#endif
axis_did_move = axis_bits;
}
#endif // FT_MOTION
#if ENABLED(BABYSTEPPING)
#define _ENABLE_AXIS(A) enable_axis(_AXIS(A))

15
Marlin/src/module/stepper.h
View file

@ -49,6 +49,10 @@
#include "stepper/speed_lookuptable.h"
#endif
#if ENABLED(FT_MOTION)
#include "ft_types.h"
#endif
//
// Estimate the amount of time the Stepper ISR will take to execute
//
@ -470,6 +474,7 @@ constexpr ena_mask_t enable_overlap[] = {
//
class Stepper {
friend class Max7219;
friend class FxdTiCtrl;
friend void stepperTask(void *);
public:
@ -817,6 +822,11 @@ class Stepper {
set_directions();
}
#if ENABLED(FT_MOTION)
// Manage the planner
static void fxdTiCtrl_BlockQueueUpdate();
#endif
#if HAS_ZV_SHAPING
static void set_shaping_damping_ratio(const AxisEnum axis, const_float_t zeta);
static float get_shaping_damping_ratio(const AxisEnum axis);
@ -848,6 +858,11 @@ class Stepper {
static void microstep_init();
#endif
#if ENABLED(FT_MOTION)
static void fxdTiCtrl_stepper(const bool applyDir, const ft_command_t command);
static void fxdTiCtrl_refreshAxisDidMove();
#endif
};
extern Stepper stepper;

4
buildroot/tests/STM32F103RC_btt
View file

@ -12,8 +12,8 @@ set -e
restore_configs
opt_set MOTHERBOARD BOARD_BTT_SKR_MINI_E3_V1_0 SERIAL_PORT 1 SERIAL_PORT_2 -1 \
X_DRIVER_TYPE TMC2209 Y_DRIVER_TYPE TMC2209 Z_DRIVER_TYPE TMC2209 E0_DRIVER_TYPE TMC2209
opt_enable PINS_DEBUGGING Z_IDLE_HEIGHT
exec_test $1 $2 "BigTreeTech SKR Mini E3 1.0 - Basic Config with TMC2209 HW Serial" "$3"
opt_enable PINS_DEBUGGING Z_IDLE_HEIGHT FT_MOTION
exec_test $1 $2 "BigTreeTech SKR Mini E3 1.0 - TMC2209 HW Serial, FT_MOTION" "$3"
# clean up
restore_configs

1
ini/features.ini
View file

@ -186,6 +186,7 @@ AIR_EVACUATION = src_filter=+<src/gcode/control/M10-M11.
HAS_SOFTWARE_ENDSTOPS = src_filter=+<src/gcode/control/M211.cpp>
SERVO_DETACH_GCODE = src_filter=+<src/gcode/control/M282.cpp>
HAS_DUPLICATION_MODE = src_filter=+<src/gcode/control/M605.cpp>
FT_MOTION = src_filter=+<src/module/ft_motion.cpp> +<src/gcode/feature/ft_motion/M493.cpp>
LIN_ADVANCE = src_filter=+<src/gcode/feature/advance>
PHOTO_GCODE = src_filter=+<src/gcode/feature/camera>
CONTROLLER_FAN_EDITABLE = src_filter=+<src/gcode/feature/controllerfan>

1
platformio.ini
View file

@ -192,6 +192,7 @@ default_src_filter = +<src/*> -<src/config> -<src/HAL> +<src/HAL/shared> -<src/t
-<src/gcode/control/M211.cpp>
-<src/gcode/control/M350_M351.cpp>
-<src/gcode/control/M605.cpp>
-<src/module/ft_motion.cpp> -<src/gcode/feature/ft_motion/M493.cpp>
-<src/gcode/feature/advance>
-<src/gcode/feature/camera>
-<src/gcode/feature/i2c>