🐛⚡️ FT_MOTION improvements (#26074)
Co-Authored-By: Scott Lahteine <thinkyhead@users.noreply.github.com>
This commit is contained in:
narno2202
Scott Lahteine
parent
0ede16cd5a
commit
67d7562609
|
|
@ -1121,42 +1121,62 @@
|
|||
#if ENABLED(FT_MOTION)
|
||||
#define FTM_DEFAULT_MODE ftMotionMode_DISABLED // 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.
|
||||
#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_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_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 FT_MOTION_MENU // Provide a MarlinUI menu to set M493 parameters
|
||||
|
||||
/**
|
||||
* Advanced configuration
|
||||
*/
|
||||
#define FTM_BATCH_SIZE 100 // Batch size for trajectory generation;
|
||||
#define FTM_WINDOW_SIZE 200 // Window size for trajectory generation.
|
||||
#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().
|
||||
#define FTM_UNIFIED_BWS // DON'T DISABLE unless you use Ulendo FBS (not implemented)
|
||||
#if ENABLED(FTM_UNIFIED_BWS)
|
||||
#define FTM_BW_SIZE 100 // Unified Window and Batch size with a ratio of 2
|
||||
#else
|
||||
#define FTM_WINDOW_SIZE 200 // Custom Window size for trajectory generation needed by Ulendo FBS
|
||||
#define FTM_BATCH_SIZE 100 // Custom Batch size for trajectory generation needed by Ulendo FBS
|
||||
#endif
|
||||
|
||||
// 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.
|
||||
#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)
|
||||
|
||||
//#define FT_MOTION_MENU // Provide a MarlinUI menu to set M493 parameters.
|
||||
// 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
|
||||
|
||||
// Use this to adjust the time required to consume the command buffer.
|
||||
// Try increasing this value if stepper motion is choppy.
|
||||
#define FTM_STEPPERCMD_BUFF_SIZE 3000 // Size of the stepper command buffers
|
||||
// (FTM_STEPS_PER_LOOP * FTM_POINTS_PER_LOOP) is a good start
|
||||
// If you run out of memory, fall back to 3000 and increase progressively
|
||||
#else
|
||||
// CoreXY motion needs a larger buffer size. These values are based on our testing.
|
||||
#define FTM_STEPPER_FS 30000
|
||||
#define FTM_STEPPERCMD_BUFF_SIZE 6000
|
||||
#endif
|
||||
|
||||
#define FTM_STEPS_PER_UNIT_TIME (FTM_STEPPER_FS / FTM_FS) // Interpolated stepper commands per unit time
|
||||
#define FTM_CTS_COMPARE_VAL (FTM_STEPS_PER_UNIT_TIME / 2) // Comparison value used in interpolation algorithm
|
||||
#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_RATIO (FTM_FS / FTM_MIN_SHAPE_FREQ) // Factor for use in FTM_ZMAX. DON'T CHANGE.
|
||||
#define FTM_ZMAX (FTM_RATIO * 2) // Maximum delays for shaping functions (even numbers only!)
|
||||
// Calculate as:
|
||||
// ZV : FTM_RATIO / 2
|
||||
// ZVD, MZV : FTM_RATIO
|
||||
// 2HEI : FTM_RATIO * 3 / 2
|
||||
// 3HEI : FTM_RATIO * 2
|
||||
#endif
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -57,9 +57,9 @@
|
|||
* 41 - Counter-Clockwise M4
|
||||
* 50 - Clockwise M5
|
||||
* 51 - Counter-Clockwise M5
|
||||
*
|
||||
*/
|
||||
**/
|
||||
void GcodeSuite::G35() {
|
||||
|
||||
DEBUG_SECTION(log_G35, "G35", DEBUGGING(LEVELING));
|
||||
|
||||
if (DEBUGGING(LEVELING)) log_machine_info();
|
||||
|
|
|
|||
|
|
@ -225,6 +225,7 @@ public:
|
|||
* There's no extra effect if you have a fixed Z probe.
|
||||
*/
|
||||
G29_TYPE GcodeSuite::G29() {
|
||||
|
||||
DEBUG_SECTION(log_G29, "G29", DEBUGGING(LEVELING));
|
||||
|
||||
// Leveling state is persistent when done manually with multiple G29 commands
|
||||
|
|
|
|||
|
|
@ -64,6 +64,7 @@ inline void echo_not_entered(const char c) { SERIAL_CHAR(c); SERIAL_ECHOLNPGM("
|
|||
* S5 Reset and disable mesh
|
||||
*/
|
||||
void GcodeSuite::G29() {
|
||||
|
||||
DEBUG_SECTION(log_G29, "G29", true);
|
||||
|
||||
// G29 Q is also available if debugging
|
||||
|
|
|
|||
|
|
@ -79,6 +79,7 @@
|
|||
* R Flag to recalculate points based on current probe offsets
|
||||
*/
|
||||
void GcodeSuite::G34() {
|
||||
|
||||
DEBUG_SECTION(log_G34, "G34", DEBUGGING(LEVELING));
|
||||
if (DEBUGGING(LEVELING)) log_machine_info();
|
||||
|
||||
|
|
|
|||
|
|
@ -26,6 +26,7 @@
|
|||
|
||||
#include "../../gcode.h"
|
||||
#include "../../../module/ft_motion.h"
|
||||
#include "../../../module/stepper.h"
|
||||
|
||||
void say_shaping() {
|
||||
// FT Enabled
|
||||
|
|
@ -39,6 +40,8 @@ void say_shaping() {
|
|||
default: break;
|
||||
case ftMotionMode_ZV: SERIAL_ECHOPGM("ZV"); break;
|
||||
case ftMotionMode_ZVD: SERIAL_ECHOPGM("ZVD"); break;
|
||||
case ftMotionMode_ZVDD: SERIAL_ECHOPGM("ZVDD"); break;
|
||||
case ftMotionMode_ZVDDD: SERIAL_ECHOPGM("ZVDDD"); break;
|
||||
case ftMotionMode_EI: SERIAL_ECHOPGM("EI"); break;
|
||||
case ftMotionMode_2HEI: SERIAL_ECHOPGM("2 Hump EI"); break;
|
||||
case ftMotionMode_3HEI: SERIAL_ECHOPGM("3 Hump EI"); break;
|
||||
|
|
@ -155,20 +158,19 @@ void GcodeSuite::M493() {
|
|||
|
||||
if (!parser.seen_any())
|
||||
flag.report_h = true;
|
||||
else
|
||||
planner.synchronize();
|
||||
|
||||
// Parse 'S' mode parameter.
|
||||
if (parser.seenval('S')) {
|
||||
const ftMotionMode_t oldmm = ftMotion.cfg.mode,
|
||||
newmm = (ftMotionMode_t)parser.value_byte();
|
||||
const ftMotionMode_t newmm = (ftMotionMode_t)parser.value_byte();
|
||||
|
||||
if (newmm != oldmm) {
|
||||
if (newmm != ftMotion.cfg.mode) {
|
||||
switch (newmm) {
|
||||
default: SERIAL_ECHOLNPGM("?Invalid control mode [S] value."); return;
|
||||
#if HAS_X_AXIS
|
||||
case ftMotionMode_ZV:
|
||||
case ftMotionMode_ZVD:
|
||||
case ftMotionMode_ZVDD:
|
||||
case ftMotionMode_ZVDDD:
|
||||
case ftMotionMode_EI:
|
||||
case ftMotionMode_2HEI:
|
||||
case ftMotionMode_3HEI:
|
||||
|
|
@ -177,11 +179,10 @@ void GcodeSuite::M493() {
|
|||
//case ftMotionMode_DISCTF:
|
||||
flag.update_n = flag.update_a = true;
|
||||
#endif
|
||||
case ftMotionMode_DISABLED:
|
||||
case ftMotionMode_DISABLED: flag.reset_ft = true;
|
||||
case ftMotionMode_ENABLED:
|
||||
ftMotion.cfg.mode = newmm;
|
||||
flag.report_h = true;
|
||||
if (oldmm == ftMotionMode_DISABLED) flag.reset_ft = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
@ -193,6 +194,7 @@ void GcodeSuite::M493() {
|
|||
if (parser.seen('P')) {
|
||||
const bool val = parser.value_bool();
|
||||
ftMotion.cfg.linearAdvEna = val;
|
||||
flag.report_h = true;
|
||||
SERIAL_ECHO_TERNARY(val, "Linear Advance ", "en", "dis", "abled.\n");
|
||||
}
|
||||
|
||||
|
|
@ -216,22 +218,16 @@ void GcodeSuite::M493() {
|
|||
if (ftMotion.cfg.modeHasShaper()) {
|
||||
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;
|
||||
#if HAS_DYNAMIC_FREQ_MM
|
||||
case dynFreqMode_Z_BASED:
|
||||
ftMotion.cfg.dynFreqMode = val;
|
||||
flag.report_h = true;
|
||||
break;
|
||||
#endif
|
||||
#if HAS_DYNAMIC_FREQ_G
|
||||
case dynFreqMode_MASS_BASED:
|
||||
ftMotion.cfg.dynFreqMode = val;
|
||||
flag.report_h = true;
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
SERIAL_ECHOLNPGM("?Invalid Dynamic Frequency Mode [D] value.");
|
||||
break;
|
||||
|
|
@ -279,6 +275,36 @@ void GcodeSuite::M493() {
|
|||
}
|
||||
#endif
|
||||
|
||||
// Parse zeta parameter (X axis).
|
||||
if (parser.seenval('I')) {
|
||||
const float val = parser.value_float();
|
||||
if (ftMotion.cfg.modeHasShaper()) {
|
||||
if (WITHIN(val, 0.01f, 1.0f)) {
|
||||
ftMotion.cfg.zeta[0] = val;
|
||||
flag.update_n = flag.update_a = true;
|
||||
}
|
||||
else
|
||||
SERIAL_ECHOLNPGM("Invalid X zeta [", AS_CHAR('I'), "] value."); // Zeta out of range.
|
||||
}
|
||||
else
|
||||
SERIAL_ECHOLNPGM("Wrong mode for zeta parameter.");
|
||||
}
|
||||
|
||||
// Parse vtol parameter (X axis).
|
||||
if (parser.seenval('Q')) {
|
||||
const float val = parser.value_float();
|
||||
if (ftMotion.cfg.modeHasShaper() && IS_EI_MODE(ftMotion.cfg.mode)) {
|
||||
if (WITHIN(val, 0.00f, 1.0f)) {
|
||||
ftMotion.cfg.vtol[0] = val;
|
||||
flag.update_a = true;
|
||||
}
|
||||
else
|
||||
SERIAL_ECHOLNPGM("Invalid X vtol [", AS_CHAR('Q'), "] value."); // VTol out of range.
|
||||
}
|
||||
else
|
||||
SERIAL_ECHOLNPGM("Wrong mode for vtol parameter.");
|
||||
}
|
||||
|
||||
#endif // HAS_X_AXIS
|
||||
|
||||
#if HAS_Y_AXIS
|
||||
|
|
@ -310,15 +336,50 @@ void GcodeSuite::M493() {
|
|||
}
|
||||
#endif
|
||||
|
||||
// Parse zeta parameter (Y axis).
|
||||
if (parser.seenval('J')) {
|
||||
const float val = parser.value_float();
|
||||
if (ftMotion.cfg.modeHasShaper()) {
|
||||
if (WITHIN(val, 0.01f, 1.0f)) {
|
||||
ftMotion.cfg.zeta[1] = val;
|
||||
flag.update_n = flag.update_a = true;
|
||||
}
|
||||
else
|
||||
SERIAL_ECHOLNPGM("Invalid Y zeta [", AS_CHAR('J'), "] value."); // Zeta Out of range
|
||||
}
|
||||
else
|
||||
SERIAL_ECHOLNPGM("Wrong mode for zeta parameter.");
|
||||
}
|
||||
|
||||
// Parse vtol parameter (Y axis).
|
||||
if (parser.seenval('R')) {
|
||||
const float val = parser.value_float();
|
||||
if (ftMotion.cfg.modeHasShaper() && IS_EI_MODE(ftMotion.cfg.mode)) {
|
||||
if (WITHIN(val, 0.00f, 1.0f)) {
|
||||
ftMotion.cfg.vtol[1] = val;
|
||||
flag.update_a = true;
|
||||
}
|
||||
else
|
||||
SERIAL_ECHOLNPGM("Invalid Y vtol [", AS_CHAR('R'), "] value."); // VTol out of range.
|
||||
}
|
||||
else
|
||||
SERIAL_ECHOLNPGM("Wrong mode for vtol parameter.");
|
||||
}
|
||||
|
||||
#endif // HAS_Y_AXIS
|
||||
|
||||
#if HAS_X_AXIS
|
||||
if (flag.update_n) ftMotion.refreshShapingN();
|
||||
if (flag.update_a) ftMotion.updateShapingA();
|
||||
#endif
|
||||
if (flag.reset_ft) ftMotion.reset();
|
||||
if (flag.report_h) say_shaping();
|
||||
planner.synchronize();
|
||||
|
||||
if (flag.update_n) ftMotion.refreshShapingN();
|
||||
|
||||
if (flag.update_a) ftMotion.updateShapingA();
|
||||
|
||||
if (flag.reset_ft) {
|
||||
stepper.ftMotion_syncPosition();
|
||||
ftMotion.reset();
|
||||
}
|
||||
|
||||
if (flag.report_h) say_shaping();
|
||||
}
|
||||
|
||||
#endif // FT_MOTION
|
||||
|
|
|
|||
|
|
@ -105,6 +105,7 @@ inline bool G38_run_probe() {
|
|||
* G38.5 - Probe away from workpiece, stop on contact break
|
||||
*/
|
||||
void GcodeSuite::G38(const int8_t subcode) {
|
||||
|
||||
// Get X Y Z E F
|
||||
get_destination_from_command();
|
||||
|
||||
|
|
|
|||
|
|
@ -1144,7 +1144,7 @@
|
|||
#elif HAS_DRIVER(A4988)
|
||||
#define MINIMUM_STEPPER_POST_DIR_DELAY 200
|
||||
#elif HAS_TRINAMIC_CONFIG || HAS_TRINAMIC_STANDALONE
|
||||
#define MINIMUM_STEPPER_POST_DIR_DELAY 60
|
||||
#define MINIMUM_STEPPER_POST_DIR_DELAY 70
|
||||
#else
|
||||
#define MINIMUM_STEPPER_POST_DIR_DELAY 0 // Expect at least 10µS since one Stepper ISR must transpire
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -802,6 +802,8 @@ namespace LanguageNarrow_en {
|
|||
LSTR MSG_FTM_MODE = _UxGT("Motion Mode:");
|
||||
LSTR MSG_FTM_ZV = _UxGT("ZV");
|
||||
LSTR MSG_FTM_ZVD = _UxGT("ZVD");
|
||||
LSTR MSG_FTM_ZVDD = _UxGT("ZVDD");
|
||||
LSTR MSG_FTM_ZVDDD = _UxGT("ZVDDD");
|
||||
LSTR MSG_FTM_EI = _UxGT("EI");
|
||||
LSTR MSG_FTM_2HEI = _UxGT("2HEI");
|
||||
LSTR MSG_FTM_3HEI = _UxGT("3HEI");
|
||||
|
|
@ -813,8 +815,8 @@ namespace LanguageNarrow_en {
|
|||
LSTR MSG_FTM_MASS_BASED = _UxGT("Mass-based");
|
||||
LSTR MSG_FTM_BASE_FREQ_N = _UxGT("@ Base Freq.");
|
||||
LSTR MSG_FTM_DFREQ_K_N = _UxGT("@ Dyn. Freq.");
|
||||
LSTR MSG_FTM_ZETA = _UxGT("Damping");
|
||||
LSTR MSG_FTM_VTOL = _UxGT("Vib. Level");
|
||||
LSTR MSG_FTM_ZETA_N = _UxGT("@ Damping");
|
||||
LSTR MSG_FTM_VTOL_N = _UxGT("@ Vib. Level");
|
||||
|
||||
LSTR MSG_LEVEL_X_AXIS = _UxGT("Level X Axis");
|
||||
LSTR MSG_AUTO_CALIBRATE = _UxGT("Auto Calibrate");
|
||||
|
|
|
|||
|
|
@ -342,6 +342,8 @@ void menu_move() {
|
|||
#if HAS_X_AXIS
|
||||
if (mode != ftMotionMode_ZV) ACTION_ITEM(MSG_FTM_ZV, []{ ftm_menu_setShaping(ftMotionMode_ZV); });
|
||||
if (mode != ftMotionMode_ZVD) ACTION_ITEM(MSG_FTM_ZVD, []{ ftm_menu_setShaping(ftMotionMode_ZVD); });
|
||||
if (mode != ftMotionMode_ZVDD) ACTION_ITEM(MSG_FTM_ZVDD, []{ ftm_menu_setShaping(ftMotionMode_ZVDD); });
|
||||
if (mode != ftMotionMode_ZVDDD) ACTION_ITEM(MSG_FTM_ZVDDD,[]{ ftm_menu_setShaping(ftMotionMode_ZVDDD); });
|
||||
if (mode != ftMotionMode_EI) ACTION_ITEM(MSG_FTM_EI, []{ ftm_menu_setShaping(ftMotionMode_EI); });
|
||||
if (mode != ftMotionMode_2HEI) ACTION_ITEM(MSG_FTM_2HEI, []{ ftm_menu_setShaping(ftMotionMode_2HEI); });
|
||||
if (mode != ftMotionMode_3HEI) ACTION_ITEM(MSG_FTM_3HEI, []{ ftm_menu_setShaping(ftMotionMode_3HEI); });
|
||||
|
|
@ -384,6 +386,8 @@ void menu_move() {
|
|||
case ftMotionMode_ENABLED: ftmode = GET_TEXT_F(MSG_LCD_ON); break;
|
||||
case ftMotionMode_ZV: ftmode = GET_TEXT_F(MSG_FTM_ZV); break;
|
||||
case ftMotionMode_ZVD: ftmode = GET_TEXT_F(MSG_FTM_ZVD); break;
|
||||
case ftMotionMode_ZVDD: ftmode = GET_TEXT_F(MSG_FTM_ZVDD); break;
|
||||
case ftMotionMode_ZVDDD: ftmode = GET_TEXT_F(MSG_FTM_ZVDDD);break;
|
||||
case ftMotionMode_EI: ftmode = GET_TEXT_F(MSG_FTM_EI); break;
|
||||
case ftMotionMode_2HEI: ftmode = GET_TEXT_F(MSG_FTM_2HEI); break;
|
||||
case ftMotionMode_3HEI: ftmode = GET_TEXT_F(MSG_FTM_3HEI); break;
|
||||
|
|
@ -416,10 +420,21 @@ void menu_move() {
|
|||
EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_BASE_FREQ_N, &c.baseFreq[Y_AXIS], FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2, ftMotion.refreshShapingN);
|
||||
#endif
|
||||
|
||||
EDIT_ITEM_FAST(float42_52, MSG_FTM_ZETA, &c.zeta, 0.0f, 1.0f, ftMotion.refreshShapingN);
|
||||
#if HAS_X_AXIS
|
||||
EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_ZETA_N, &c.zeta[0], 0.0f, 1.0f, ftMotion.refreshShapingN);
|
||||
#endif
|
||||
#if HAS_Y_AXIS
|
||||
EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_ZETA_N, &c.zeta[1], 0.0f, 1.0f, ftMotion.refreshShapingN);
|
||||
#endif
|
||||
|
||||
if (WITHIN(c.mode, ftMotionMode_EI, ftMotionMode_3HEI))
|
||||
EDIT_ITEM_FAST(float42_52, MSG_FTM_VTOL, &c.vtol, 0.0f, 1.0f, ftMotion.refreshShapingN);
|
||||
if (IS_EI_MODE(c.mode)) {
|
||||
#if HAS_X_AXIS
|
||||
EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_VTOL_N, &c.vtol[0], 0.0f, 1.0f, ftMotion.refreshShapingN);
|
||||
#endif
|
||||
#if HAS_Y_AXIS
|
||||
EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_VTOL_N, &c.vtol[1], 0.0f, 1.0f, ftMotion.refreshShapingN);
|
||||
#endif
|
||||
}
|
||||
|
||||
#if HAS_DYNAMIC_FREQ
|
||||
SUBMENU(MSG_FTM_DYN_MODE, menu_ftm_dyn_mode);
|
||||
|
|
@ -437,7 +452,7 @@ void menu_move() {
|
|||
|
||||
#if HAS_EXTRUDERS
|
||||
EDIT_ITEM(bool, MSG_LINEAR_ADVANCE, &c.linearAdvEna);
|
||||
EDIT_ITEM(float42_52, MSG_ADVANCE_K, &c.linearAdvK, 0, 10);
|
||||
if (c.linearAdvEna) EDIT_ITEM(float42_52, MSG_ADVANCE_K, &c.linearAdvK, 0, 10);
|
||||
#endif
|
||||
|
||||
END_MENU();
|
||||
|
|
|
|||
|
|
@ -32,6 +32,8 @@ FTMotion ftMotion;
|
|||
#if !HAS_X_AXIS
|
||||
static_assert(FTM_DEFAULT_MODE == ftMotionMode_ZV, "ftMotionMode_ZV requires at least one linear axis.");
|
||||
static_assert(FTM_DEFAULT_MODE == ftMotionMode_ZVD, "ftMotionMode_ZVD requires at least one linear axis.");
|
||||
static_assert(FTM_DEFAULT_MODE == ftMotionMode_ZVDD, "ftMotionMode_ZVD requires at least one linear axis.");
|
||||
static_assert(FTM_DEFAULT_MODE == ftMotionMode_ZVDDD, "ftMotionMode_ZVD requires at least one linear axis.");
|
||||
static_assert(FTM_DEFAULT_MODE == ftMotionMode_EI, "ftMotionMode_EI requires at least one linear axis.");
|
||||
static_assert(FTM_DEFAULT_MODE == ftMotionMode_2HEI, "ftMotionMode_2HEI requires at least one linear axis.");
|
||||
static_assert(FTM_DEFAULT_MODE == ftMotionMode_3HEI, "ftMotionMode_3HEI requires at least one linear axis.");
|
||||
|
|
@ -52,9 +54,7 @@ FTMotion ftMotion;
|
|||
|
||||
ft_config_t FTMotion::cfg;
|
||||
bool FTMotion::busy; // = false
|
||||
ft_command_t FTMotion::stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE] = {0U}; // Buffer of stepper commands.
|
||||
hal_timer_t FTMotion::stepperCmdBuff_StepRelativeTi[FTM_STEPPERCMD_BUFF_SIZE] = {0U}; // Buffer of the stepper command timing.
|
||||
uint8_t FTMotion::stepperCmdBuff_ApplyDir[FTM_STEPPERCMD_DIR_SIZE] = {0U}; // Buffer of whether DIR needs to be updated.
|
||||
ft_command_t FTMotion::stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE] = {0U}; // Stepper commands buffer.
|
||||
uint32_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.
|
||||
|
||||
|
|
@ -62,10 +62,10 @@ bool FTMotion::sts_stepperBusy = false; // The stepper buffer has items
|
|||
|
||||
// Private variables.
|
||||
// NOTE: These are sized for Ulendo FBS use.
|
||||
xyze_trajectory_t FTMotion::traj; // = {0.0f} Storage for fixed-time-based trajectory.
|
||||
xyze_trajectoryMod_t FTMotion::trajMod; // = {0.0f} Storage for modified fixed-time-based trajectory.
|
||||
xyze_trajectory_t FTMotion::traj; // = {0.0f} Storage for fixed-time-based trajectory.
|
||||
xyze_trajectoryMod_t FTMotion::trajMod; // = {0.0f} Storage for modified fixed-time-based trajectory.
|
||||
xyze_trajectoryWin_t FTMotion::trajWin; // = {0.0f} Storage for fixed time trajectory window.
|
||||
|
||||
block_t* FTMotion::current_block_cpy = nullptr; // Pointer to current block being processed.
|
||||
bool FTMotion::blockProcRdy = false, // Indicates a block is ready to be processed.
|
||||
FTMotion::blockProcRdy_z1 = false, // Storage for the previous indicator.
|
||||
FTMotion::blockProcDn = false; // Indicates current block is done being processed.
|
||||
|
|
@ -76,7 +76,7 @@ bool FTMotion::batchRdy = false; // Indicates a batch of the fixe
|
|||
bool FTMotion::batchRdyForInterp = false; // Indicates the batch is done being post processed,
|
||||
// if applicable, and is ready to be converted to step commands.
|
||||
bool FTMotion::runoutEna = false; // True if runout of the block hasn't been done and is allowed.
|
||||
bool FTMotion::runout = false; // Indicates if runout is in progress.
|
||||
bool FTMotion::blockDataIsRunout = false; // Indicates the last loaded block variables are for a runout.
|
||||
|
||||
// Trapezoid data variables.
|
||||
xyze_pos_t FTMotion::startPosn, // (mm) Start position of block
|
||||
|
|
@ -98,15 +98,13 @@ uint32_t FTMotion::max_intervals; // Total number of data points t
|
|||
// Make vector variables.
|
||||
uint32_t FTMotion::makeVector_idx = 0, // Index of fixed time trajectory generation of the overall block.
|
||||
FTMotion::makeVector_idx_z1 = 0, // Storage for the previously calculated index above.
|
||||
FTMotion::makeVector_batchIdx = FTM_BATCH_SIZE; // Index of fixed time trajectory generation within the batch.
|
||||
FTMotion::makeVector_batchIdx = 0; // Index of fixed time trajectory generation within the batch.
|
||||
|
||||
// Interpolation variables.
|
||||
xyze_long_t FTMotion::steps = { 0 }; // Step count accumulator.
|
||||
xyze_stepDir_t FTMotion::dirState = LOGICAL_AXIS_ARRAY_1(stepDirState_NOT_SET); // Memory of the currently set step direction of the axis.
|
||||
|
||||
uint32_t FTMotion::interpIdx = 0, // Index of current data point being interpolated.
|
||||
FTMotion::interpIdx_z1 = 0; // Storage for the previously calculated index above.
|
||||
hal_timer_t FTMotion::nextStepTicks = FTM_MIN_TICKS; // Accumulator for the next step time (in ticks).
|
||||
|
||||
// Shaping variables.
|
||||
#if HAS_X_AXIS
|
||||
|
|
@ -125,8 +123,6 @@ hal_timer_t FTMotion::nextStepTicks = FTM_MIN_TICKS; // Accumulator for the nex
|
|||
float FTMotion::e_advanced_z1 = 0.0f; // (ms) Unit delay of advanced extruder position.
|
||||
#endif
|
||||
|
||||
constexpr uint32_t last_batchIdx = (FTM_WINDOW_SIZE) - (FTM_BATCH_SIZE);
|
||||
|
||||
//-----------------------------------------------------------------//
|
||||
// Function definitions.
|
||||
//-----------------------------------------------------------------//
|
||||
|
|
@ -134,8 +130,7 @@ constexpr uint32_t last_batchIdx = (FTM_WINDOW_SIZE) - (FTM_BATCH_SIZE);
|
|||
// Public functions.
|
||||
|
||||
// Sets controller states to begin processing a block.
|
||||
void FTMotion::startBlockProc(block_t * const current_block) {
|
||||
current_block_cpy = current_block;
|
||||
void FTMotion::startBlockProc() {
|
||||
blockProcRdy = true;
|
||||
blockProcDn = false;
|
||||
runoutEna = true;
|
||||
|
|
@ -144,30 +139,16 @@ void FTMotion::startBlockProc(block_t * const current_block) {
|
|||
// Move any free data points to the stepper buffer even if a full batch isn't ready.
|
||||
void FTMotion::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 > last_batchIdx)
|
||||
for (uint32_t i = makeVector_batchIdx; i < (FTM_WINDOW_SIZE); i++) {
|
||||
LOGICAL_AXIS_CODE(
|
||||
traj.e[i] = traj.e[makeVector_batchIdx - 1],
|
||||
traj.x[i] = traj.x[makeVector_batchIdx - 1],
|
||||
traj.y[i] = traj.y[makeVector_batchIdx - 1],
|
||||
traj.z[i] = traj.z[makeVector_batchIdx - 1],
|
||||
traj.i[i] = traj.i[makeVector_batchIdx - 1],
|
||||
traj.j[i] = traj.j[makeVector_batchIdx - 1],
|
||||
traj.k[i] = traj.k[makeVector_batchIdx - 1],
|
||||
traj.u[i] = traj.u[makeVector_batchIdx - 1],
|
||||
traj.v[i] = traj.v[makeVector_batchIdx - 1],
|
||||
traj.w[i] = traj.w[makeVector_batchIdx - 1]
|
||||
);
|
||||
}
|
||||
if (!runoutEna) return;
|
||||
|
||||
makeVector_batchIdx = last_batchIdx;
|
||||
batchRdy = true;
|
||||
runout = true;
|
||||
}
|
||||
runoutEna = false;
|
||||
startPosn = endPosn_prevBlock;
|
||||
ratio.reset();
|
||||
|
||||
max_intervals = cfg.modeHasShaper() ? shaper_intervals : 0;
|
||||
if (max_intervals <= TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, min_max_intervals - (FTM_BATCH_SIZE))) max_intervals = min_max_intervals;
|
||||
max_intervals += TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, FTM_WINDOW_SIZE) - makeVector_batchIdx;
|
||||
blockProcRdy = blockDataIsRunout = true;
|
||||
runoutEna = blockProcDn = false;
|
||||
}
|
||||
|
||||
// Controller main, to be invoked from non-isr task.
|
||||
|
|
@ -184,81 +165,63 @@ void FTMotion::loop() {
|
|||
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 && !runout) stepper.ftMotion_BlockQueueUpdate();
|
||||
if (!blockProcRdy) stepper.ftMotion_blockQueueUpdate();
|
||||
|
||||
if (blockProcRdy) {
|
||||
if (!blockProcRdy_z1) loadBlockData(current_block_cpy); // One-shot.
|
||||
if (!blockProcRdy_z1) { // One-shot.
|
||||
if (!blockDataIsRunout) loadBlockData(stepper.current_block);
|
||||
else blockDataIsRunout = false;
|
||||
}
|
||||
while (!blockProcDn && !batchRdy && (makeVector_idx - makeVector_idx_z1 < (FTM_POINTS_PER_LOOP)))
|
||||
makeVector();
|
||||
}
|
||||
|
||||
if (runout && !batchRdy) { // The lower half of the window has been runout.
|
||||
// Runout the upper half of the window: the upper half has been shifted into the lower
|
||||
// half. Fill out the upper half so another batch can be processed.
|
||||
for (uint32_t i = last_batchIdx; i < (FTM_WINDOW_SIZE) - 1; i++) {
|
||||
LOGICAL_AXIS_CODE(
|
||||
traj.e[i] = traj.e[(FTM_WINDOW_SIZE) - 1],
|
||||
traj.x[i] = traj.x[(FTM_WINDOW_SIZE) - 1],
|
||||
traj.y[i] = traj.y[(FTM_WINDOW_SIZE) - 1],
|
||||
traj.z[i] = traj.z[(FTM_WINDOW_SIZE) - 1],
|
||||
traj.i[i] = traj.i[(FTM_WINDOW_SIZE) - 1],
|
||||
traj.j[i] = traj.j[(FTM_WINDOW_SIZE) - 1],
|
||||
traj.k[i] = traj.k[(FTM_WINDOW_SIZE) - 1],
|
||||
traj.u[i] = traj.u[(FTM_WINDOW_SIZE) - 1],
|
||||
traj.v[i] = traj.v[(FTM_WINDOW_SIZE) - 1],
|
||||
traj.w[i] = traj.w[(FTM_WINDOW_SIZE) - 1]
|
||||
);
|
||||
}
|
||||
batchRdy = true;
|
||||
runout = false;
|
||||
}
|
||||
|
||||
// FBS / post processing.
|
||||
if (batchRdy && !batchRdyForInterp) {
|
||||
|
||||
// Call Ulendo FBS here.
|
||||
|
||||
// Copy the uncompensated vectors. (XY done, other axes uncompensated)
|
||||
LOGICAL_AXIS_CODE(
|
||||
memcpy(trajMod.e, &traj.e[FTM_BATCH_SIZE], sizeof(trajMod.e)),
|
||||
memcpy(trajMod.x, &traj.x[FTM_BATCH_SIZE], sizeof(trajMod.x)),
|
||||
memcpy(trajMod.y, &traj.y[FTM_BATCH_SIZE], sizeof(trajMod.y)),
|
||||
memcpy(trajMod.z, &traj.z[FTM_BATCH_SIZE], sizeof(trajMod.z)),
|
||||
memcpy(trajMod.i, &traj.i[FTM_BATCH_SIZE], sizeof(trajMod.i)),
|
||||
memcpy(trajMod.j, &traj.j[FTM_BATCH_SIZE], sizeof(trajMod.j)),
|
||||
memcpy(trajMod.k, &traj.k[FTM_BATCH_SIZE], sizeof(trajMod.k)),
|
||||
memcpy(trajMod.u, &traj.u[FTM_BATCH_SIZE], sizeof(trajMod.u)),
|
||||
memcpy(trajMod.v, &traj.v[FTM_BATCH_SIZE], sizeof(trajMod.v)),
|
||||
memcpy(trajMod.w, &traj.w[FTM_BATCH_SIZE], sizeof(trajMod.w))
|
||||
);
|
||||
#if ENABLED(FTM_UNIFIED_BWS)
|
||||
trajMod = traj; // Copy the uncompensated vectors.
|
||||
traj = trajWin; // Move the window to traj
|
||||
#else
|
||||
// Copy the uncompensated vectors.
|
||||
#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 for (shaped) X and Y
|
||||
TERN_(HAS_X_AXIS, memcpy(traj.x, &traj.x[FTM_BATCH_SIZE], sizeof(traj.x) / 2));
|
||||
TERN_(HAS_Y_AXIS, memcpy(traj.y, &traj.y[FTM_BATCH_SIZE], sizeof(traj.y) / 2));
|
||||
|
||||
// Z...W and E Disabled! Uncompensated so the lower half is not used.
|
||||
//TERN_(HAS_Z_AXIS, memcpy(&traj.z[0], &traj.z[FTM_BATCH_SIZE], sizeof(traj.z) / 2));
|
||||
// Shift the time series back in the window
|
||||
#define TSHIFT(A) memcpy(traj.A, trajWin.A, sizeof(trajWin.A))
|
||||
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;
|
||||
|
||||
batchRdy = false; // Clear so that makeVector() may resume generating points.
|
||||
|
||||
} // if (batchRdy && !batchRdyForInterp)
|
||||
batchRdy = false; // Clear so makeVector() can resume generating points.
|
||||
}
|
||||
|
||||
// Interpolation.
|
||||
while ( batchRdyForInterp
|
||||
&& ( stepperCmdBuffItems() < ((FTM_STEPPERCMD_BUFF_SIZE) - (FTM_STEPS_PER_UNIT_TIME)) )
|
||||
&& ( (interpIdx - interpIdx_z1) < (FTM_STEPS_PER_LOOP) )
|
||||
&& ( stepperCmdBuffItems() < (FTM_STEPPERCMD_BUFF_SIZE) - (FTM_STEPS_PER_UNIT_TIME) )
|
||||
&& ( interpIdx - interpIdx_z1 < (FTM_STEPS_PER_LOOP) )
|
||||
) {
|
||||
convertToSteps(interpIdx);
|
||||
|
||||
if (++interpIdx == FTM_BATCH_SIZE) {
|
||||
if (++interpIdx == TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, FTM_BATCH_SIZE)) {
|
||||
batchRdyForInterp = false;
|
||||
interpIdx = 0;
|
||||
}
|
||||
|
|
@ -277,76 +240,153 @@ void FTMotion::loop() {
|
|||
// Refresh the gains used by shaping functions.
|
||||
// To be called on init or mode or zeta change.
|
||||
|
||||
void FTMotion::Shaping::updateShapingA(const_float_t zeta/*=cfg.zeta*/, const_float_t vtol/*=cfg.vtol*/) {
|
||||
void FTMotion::Shaping::updateShapingA(float zeta[]/*=cfg.zeta*/, float vtol[]/*=cfg.vtol*/) {
|
||||
|
||||
const float K = exp(-zeta * M_PI / sqrt(1.0f - sq(zeta))),
|
||||
K2 = sq(K);
|
||||
const float Kx = exp(-zeta[0] * M_PI / sqrt(1.0f - sq(zeta[0]))),
|
||||
Ky = exp(-zeta[1] * M_PI / sqrt(1.0f - sq(zeta[1]))),
|
||||
Kx2 = sq(Kx),
|
||||
Ky2 = sq(Ky);
|
||||
|
||||
switch (cfg.mode) {
|
||||
|
||||
case ftMotionMode_ZV:
|
||||
max_i = 1U;
|
||||
x.Ai[0] = 1.0f / (1.0f + K);
|
||||
x.Ai[1] = x.Ai[0] * K;
|
||||
x.Ai[0] = 1.0f / (1.0f + Kx);
|
||||
x.Ai[1] = x.Ai[0] * Kx;
|
||||
|
||||
y.Ai[0] = 1.0f / (1.0f + Ky);
|
||||
y.Ai[1] = y.Ai[0] * Ky;
|
||||
break;
|
||||
|
||||
case ftMotionMode_ZVD:
|
||||
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;
|
||||
x.Ai[0] = 1.0f / (1.0f + 2.0f * Kx + Kx2);
|
||||
x.Ai[1] = x.Ai[0] * 2.0f * Kx;
|
||||
x.Ai[2] = x.Ai[0] * Kx2;
|
||||
|
||||
y.Ai[0] = 1.0f / (1.0f + 2.0f * Ky + Ky2);
|
||||
y.Ai[1] = y.Ai[0] * 2.0f * Ky;
|
||||
y.Ai[2] = y.Ai[0] * Ky2;
|
||||
break;
|
||||
|
||||
case ftMotionMode_ZVDD:
|
||||
max_i = 3U;
|
||||
x.Ai[0] = 1.0f / (1.0f + 3.0f * Kx + 3.0f * Kx2 + cu(Kx));
|
||||
x.Ai[1] = x.Ai[0] * 3.0f * Kx;
|
||||
x.Ai[2] = x.Ai[0] * 3.0f * Kx2;
|
||||
x.Ai[3] = x.Ai[0] * cu(Kx);
|
||||
|
||||
y.Ai[0] = 1.0f / (1.0f + 3.0f * Ky + 3.0f * Ky2 + cu(Ky));
|
||||
y.Ai[1] = y.Ai[0] * 3.0f * Ky;
|
||||
y.Ai[2] = y.Ai[0] * 3.0f * Ky2;
|
||||
y.Ai[3] = y.Ai[0] * cu(Ky);
|
||||
break;
|
||||
|
||||
case ftMotionMode_ZVDDD:
|
||||
max_i = 4U;
|
||||
x.Ai[0] = 1.0f / (1.0f + 4.0f * Kx + 6.0f * Kx2 + 4.0f * cu(Kx) + sq(Kx2));
|
||||
x.Ai[1] = x.Ai[0] * 4.0f * Kx;
|
||||
x.Ai[2] = x.Ai[0] * 6.0f * Kx2;
|
||||
x.Ai[3] = x.Ai[0] * 4.0f * cu(Kx);
|
||||
x.Ai[4] = x.Ai[0] * sq(Kx2);
|
||||
|
||||
y.Ai[0] = 1.0f / (1.0f + 4.0f * Ky + 6.0f * Ky2 + 4.0f * cu(Ky) + sq(Ky2));
|
||||
y.Ai[1] = y.Ai[0] * 4.0f * Ky;
|
||||
y.Ai[2] = y.Ai[0] * 6.0f * Ky2;
|
||||
y.Ai[3] = y.Ai[0] * 4.0f * cu(Ky);
|
||||
y.Ai[4] = y.Ai[0] * sq(Ky2);
|
||||
break;
|
||||
|
||||
case ftMotionMode_EI: {
|
||||
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;
|
||||
x.Ai[0] = 0.25f * (1.0f + vtol[0]);
|
||||
x.Ai[1] = 0.50f * (1.0f - vtol[0]) * Kx;
|
||||
x.Ai[2] = x.Ai[0] * Kx2;
|
||||
|
||||
y.Ai[0] = 0.25f * (1.0f + vtol[1]);
|
||||
y.Ai[1] = 0.50f * (1.0f - vtol[1]) * Ky;
|
||||
y.Ai[2] = y.Ai[0] * Ky2;
|
||||
|
||||
const float X_adj = 1.0f / (x.Ai[0] + x.Ai[1] + x.Ai[2]);
|
||||
const float Y_adj = 1.0f / (y.Ai[0] + y.Ai[1] + y.Ai[2]);
|
||||
for (uint32_t i = 0U; i < 3U; i++) {
|
||||
x.Ai[i] *= X_adj;
|
||||
y.Ai[i] *= Y_adj;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case ftMotionMode_2HEI: {
|
||||
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;
|
||||
const float vtolx2 = sq(vtol[0]);
|
||||
const float X = pow(vtolx2 * (sqrt(1.0f - vtolx2) + 1.0f), 1.0f / 3.0f);
|
||||
x.Ai[0] = (3.0f * sq(X) + 2.0f * X + 3.0f * vtolx2) / (16.0f * X);
|
||||
x.Ai[1] = (0.5f - x.Ai[0]) * Kx;
|
||||
x.Ai[2] = x.Ai[1] * Kx;
|
||||
x.Ai[3] = x.Ai[0] * cu(Kx);
|
||||
|
||||
const float vtoly2 = sq(vtol[1]);
|
||||
const float Y = pow(vtoly2 * (sqrt(1.0f - vtoly2) + 1.0f), 1.0f / 3.0f);
|
||||
y.Ai[0] = (3.0f * sq(Y) + 2.0f * Y + 3.0f * vtoly2) / (16.0f * Y);
|
||||
y.Ai[1] = (0.5f - y.Ai[0]) * Ky;
|
||||
y.Ai[2] = y.Ai[1] * Ky;
|
||||
y.Ai[3] = y.Ai[0] * cu(Ky);
|
||||
|
||||
const float X_adj = 1.0f / (x.Ai[0] + x.Ai[1] + x.Ai[2] + x.Ai[3]);
|
||||
const float Y_adj = 1.0f / (y.Ai[0] + y.Ai[1] + y.Ai[2] + y.Ai[3]);
|
||||
for (uint32_t i = 0U; i < 4U; i++) {
|
||||
x.Ai[i] *= X_adj;
|
||||
y.Ai[i] *= Y_adj;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case ftMotionMode_3HEI: {
|
||||
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;
|
||||
x.Ai[0] = 0.0625f * ( 1.0f + 3.0f * vtol[0] + 2.0f * sqrt( 2.0f * ( vtol[0] + 1.0f ) * vtol[0] ) );
|
||||
x.Ai[1] = 0.25f * ( 1.0f - vtol[0] ) * Kx;
|
||||
x.Ai[2] = ( 0.5f * ( 1.0f + vtol[0] ) - 2.0f * x.Ai[0] ) * Kx2;
|
||||
x.Ai[3] = x.Ai[1] * Kx2;
|
||||
x.Ai[4] = x.Ai[0] * sq(Kx2);
|
||||
|
||||
y.Ai[0] = 0.0625f * (1.0f + 3.0f * vtol[1] + 2.0f * sqrt(2.0f * (vtol[1] + 1.0f) * vtol[1]));
|
||||
y.Ai[1] = 0.25f * (1.0f - vtol[1]) * Ky;
|
||||
y.Ai[2] = (0.5f * (1.0f + vtol[1]) - 2.0f * y.Ai[0]) * Ky2;
|
||||
y.Ai[3] = y.Ai[1] * Ky2;
|
||||
y.Ai[4] = y.Ai[0] * sq(Ky2);
|
||||
|
||||
const float X_adj = 1.0f / (x.Ai[0] + x.Ai[1] + x.Ai[2] + x.Ai[3] + x.Ai[4]);
|
||||
const float Y_adj = 1.0f / (y.Ai[0] + y.Ai[1] + y.Ai[2] + y.Ai[3] + y.Ai[4]);
|
||||
for (uint32_t i = 0U; i < 5U; i++) {
|
||||
x.Ai[i] *= X_adj;
|
||||
y.Ai[i] *= Y_adj;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case ftMotionMode_MZV: {
|
||||
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;
|
||||
const float Bx = 1.4142135623730950488016887242097f * Kx;
|
||||
x.Ai[0] = 1.0f / (1.0f + Bx + Kx2);
|
||||
x.Ai[1] = x.Ai[0] * Bx;
|
||||
x.Ai[2] = x.Ai[0] * Kx2;
|
||||
|
||||
const float By = 1.4142135623730950488016887242097f * Ky;
|
||||
y.Ai[0] = 1.0f / (1.0f + By + Ky2);
|
||||
y.Ai[1] = y.Ai[0] * By;
|
||||
y.Ai[2] = y.Ai[0] * Ky2;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
for (uint32_t i = 0U; i < 5U; i++) x.Ai[i] = 0.0f;
|
||||
ZERO(x.Ai);
|
||||
ZERO(y.Ai);
|
||||
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
|
||||
|
||||
}
|
||||
|
||||
void FTMotion::updateShapingA(const_float_t zeta/*=cfg.zeta*/, const_float_t vtol/*=cfg.vtol*/) {
|
||||
void FTMotion::updateShapingA(float zeta[]/*=cfg.zeta*/, float vtol[]/*=cfg.vtol*/) {
|
||||
shaping.updateShapingA(zeta, vtol);
|
||||
}
|
||||
|
||||
|
|
@ -364,11 +404,13 @@ void FTMotion::loop() {
|
|||
Ni[1] = round((0.5f / f / df) * (FTM_FS));
|
||||
Ni[2] = Ni[1] + Ni[1];
|
||||
break;
|
||||
case ftMotionMode_ZVDD:
|
||||
case ftMotionMode_2HEI:
|
||||
Ni[1] = round((0.5f / f / df) * (FTM_FS));
|
||||
Ni[2] = Ni[1] + Ni[1];
|
||||
Ni[3] = Ni[2] + Ni[1];
|
||||
break;
|
||||
case ftMotionMode_ZVDDD:
|
||||
case ftMotionMode_3HEI:
|
||||
Ni[1] = round((0.5f / f / df) * (FTM_FS));
|
||||
Ni[2] = Ni[1] + Ni[1];
|
||||
|
|
@ -383,10 +425,14 @@ void FTMotion::loop() {
|
|||
}
|
||||
}
|
||||
|
||||
void FTMotion::updateShapingN(const_float_t xf OPTARG(HAS_Y_AXIS, const_float_t yf), const_float_t zeta/*=cfg.zeta*/) {
|
||||
const float df = sqrt(1.0f - sq(zeta));
|
||||
shaping.x.updateShapingN(xf, df);
|
||||
TERN_(HAS_Y_AXIS, shaping.y.updateShapingN(yf, df));
|
||||
void FTMotion::updateShapingN(const_float_t xf OPTARG(HAS_Y_AXIS, const_float_t yf), float zeta[]/*=cfg.zeta*/) {
|
||||
const float xdf = sqrt(1.0f - sq(zeta[0]));
|
||||
shaping.x.updateShapingN(xf, xdf);
|
||||
|
||||
#if HAS_Y_AXIS
|
||||
const float ydf = sqrt(1.0f - sq(zeta[1]));
|
||||
shaping.y.updateShapingN(yf, ydf);
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif // HAS_X_AXIS
|
||||
|
|
@ -396,27 +442,24 @@ void FTMotion::reset() {
|
|||
|
||||
stepperCmdBuff_produceIdx = stepperCmdBuff_consumeIdx = 0;
|
||||
|
||||
traj.reset(); // Reset trajectory history
|
||||
trajMod.reset(); // Reset modified trajectory history
|
||||
traj.reset();
|
||||
trajWin.reset();
|
||||
|
||||
blockProcRdy = blockProcRdy_z1 = blockProcDn = false;
|
||||
batchRdy = batchRdyForInterp = false;
|
||||
runoutEna = false;
|
||||
runout = false;
|
||||
|
||||
endPosn_prevBlock.reset();
|
||||
|
||||
makeVector_idx = makeVector_idx_z1 = 0;
|
||||
makeVector_batchIdx = FTM_BATCH_SIZE;
|
||||
makeVector_batchIdx = 0;
|
||||
|
||||
steps.reset();
|
||||
interpIdx = interpIdx_z1 = 0;
|
||||
dirState = LOGICAL_AXIS_ARRAY_1(stepDirState_NOT_SET);
|
||||
nextStepTicks = FTM_MIN_TICKS;
|
||||
|
||||
#if HAS_X_AXIS
|
||||
for (uint32_t i = 0U; i < (FTM_ZMAX); i++)
|
||||
shaping.x.d_zi[i] = TERN_(HAS_Y_AXIS, shaping.y.d_zi[i] =) 0.0f;
|
||||
ZERO(shaping.x.d_zi);
|
||||
TERN_(HAS_Y_AXIS, ZERO(shaping.y.d_zi));
|
||||
shaping.zi_idx = 0;
|
||||
#endif
|
||||
|
||||
|
|
@ -445,40 +488,26 @@ void FTMotion::loadBlockData(block_t * const current_block) {
|
|||
const float totalLength = current_block->millimeters,
|
||||
oneOverLength = 1.0f / totalLength;
|
||||
|
||||
const AxisBits direction = current_block->direction_bits;
|
||||
|
||||
startPosn = endPosn_prevBlock;
|
||||
xyze_pos_t moveDist = LOGICAL_AXIS_ARRAY(
|
||||
current_block->steps.e / planner.settings.axis_steps_per_mm[E_AXIS_N(current_block->extruder)],
|
||||
current_block->steps.x / planner.settings.axis_steps_per_mm[X_AXIS],
|
||||
current_block->steps.y / planner.settings.axis_steps_per_mm[Y_AXIS],
|
||||
current_block->steps.z / planner.settings.axis_steps_per_mm[Z_AXIS],
|
||||
current_block->steps.i / planner.settings.axis_steps_per_mm[I_AXIS],
|
||||
current_block->steps.j / planner.settings.axis_steps_per_mm[J_AXIS],
|
||||
current_block->steps.k / planner.settings.axis_steps_per_mm[K_AXIS],
|
||||
current_block->steps.u / planner.settings.axis_steps_per_mm[U_AXIS],
|
||||
current_block->steps.v / planner.settings.axis_steps_per_mm[V_AXIS],
|
||||
current_block->steps.w / planner.settings.axis_steps_per_mm[W_AXIS]
|
||||
);
|
||||
|
||||
LOGICAL_AXIS_CODE(
|
||||
if (!direction.e) moveDist.e *= -1.0f,
|
||||
if (!direction.x) moveDist.x *= -1.0f,
|
||||
if (!direction.y) moveDist.y *= -1.0f,
|
||||
if (!direction.z) moveDist.z *= -1.0f,
|
||||
if (!direction.i) moveDist.i *= -1.0f,
|
||||
if (!direction.j) moveDist.j *= -1.0f,
|
||||
if (!direction.k) moveDist.k *= -1.0f,
|
||||
if (!direction.u) moveDist.u *= -1.0f,
|
||||
if (!direction.v) moveDist.v *= -1.0f,
|
||||
if (!direction.w) moveDist.w *= -1.0f
|
||||
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),
|
||||
current_block->steps.z * planner.mm_per_step[Z_AXIS] * (current_block->direction_bits.z ? 1 : -1),
|
||||
current_block->steps.i * planner.mm_per_step[I_AXIS] * (current_block->direction_bits.i ? 1 : -1),
|
||||
current_block->steps.j * planner.mm_per_step[J_AXIS] * (current_block->direction_bits.j ? 1 : -1),
|
||||
current_block->steps.k * planner.mm_per_step[K_AXIS] * (current_block->direction_bits.k ? 1 : -1),
|
||||
current_block->steps.u * planner.mm_per_step[U_AXIS] * (current_block->direction_bits.u ? 1 : -1),
|
||||
current_block->steps.v * planner.mm_per_step[V_AXIS] * (current_block->direction_bits.v ? 1 : -1),
|
||||
current_block->steps.w * planner.mm_per_step[W_AXIS] * (current_block->direction_bits.w ? 1 : -1)
|
||||
);
|
||||
|
||||
ratio = moveDist * oneOverLength;
|
||||
|
||||
/* Keep for comprehension
|
||||
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
|
||||
f_s = spm * current_block->initial_rate, // (steps/s) Start feedrate
|
||||
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
|
||||
|
|
@ -499,25 +528,46 @@ void FTMotion::loadBlockData(block_t * const current_block) {
|
|||
|
||||
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 spm = totalLength / current_block->step_event_count,
|
||||
f_s = spm * current_block->initial_rate,
|
||||
f_e = spm * current_block->final_rate;
|
||||
|
||||
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
|
||||
const float accel = current_block->acceleration,
|
||||
oneOverAccel = 1.0f / accel;
|
||||
|
||||
// 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
|
||||
//
|
||||
float F_n = current_block->nominal_speed;
|
||||
const float ldiff = totalLength + 0.5f * oneOverAccel * (sq(f_s) + sq(f_e));
|
||||
|
||||
float T2 = ldiff / F_n - oneOverAccel * F_n;
|
||||
if (T2 < 0.0f) {
|
||||
T2 = 0.0f;
|
||||
F_n = SQRT(ldiff * accel);
|
||||
}
|
||||
|
||||
const float T1 = (F_n - f_s) * oneOverAccel,
|
||||
T3 = (F_n - f_e) * oneOverAccel;
|
||||
|
||||
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 : (mm/s) Starting feedrate
|
||||
* f_e : (mm/s) Ending feedrate
|
||||
* T1_P : (sec) Time spent accelerating
|
||||
* T2_P : (sec) Time spent coasting
|
||||
* T3_P : (sec) Time spent decelerating
|
||||
* f_s * T1_P : (mm) Distance traveled during the accel phase
|
||||
* f_e * T3_P : (mm) 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
|
||||
|
|
@ -531,77 +581,69 @@ void FTMotion::loadBlockData(block_t * const current_block) {
|
|||
// 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;
|
||||
// Accel + Coasting + Decel datapoints
|
||||
max_intervals = N1 + N2 + N3;
|
||||
|
||||
endPosn_prevBlock += moveDist;
|
||||
}
|
||||
|
||||
// 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
|
||||
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
|
||||
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 && makeVector_idx < (N1 + N2)) {
|
||||
else if (makeVector_idx < (N1 + N2)) {
|
||||
// Coasting phase
|
||||
dist = s_1e + F_P * (tau - N1 * (FTM_TS)); // (mm) Distance traveled for 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
|
||||
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
|
||||
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
|
||||
}
|
||||
|
||||
NUM_AXIS_CODE(
|
||||
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
|
||||
LOGICAL_AXIS_CODE(
|
||||
trajWin.e[makeVector_batchIdx] = startPosn.e + ratio.e * dist,
|
||||
trajWin.x[makeVector_batchIdx] = startPosn.x + ratio.x * dist,
|
||||
trajWin.y[makeVector_batchIdx] = startPosn.y + ratio.y * dist,
|
||||
trajWin.z[makeVector_batchIdx] = startPosn.z + ratio.z * dist,
|
||||
trajWin.i[makeVector_batchIdx] = startPosn.i + ratio.i * dist,
|
||||
trajWin.j[makeVector_batchIdx] = startPosn.j + ratio.j * dist,
|
||||
trajWin.k[makeVector_batchIdx] = startPosn.k + ratio.k * dist,
|
||||
trajWin.u[makeVector_batchIdx] = startPosn.u + ratio.u * dist,
|
||||
trajWin.v[makeVector_batchIdx] = startPosn.v + ratio.v * dist,
|
||||
trajWin.w[makeVector_batchIdx] = startPosn.w + ratio.w * dist
|
||||
);
|
||||
|
||||
#if HAS_EXTRUDERS
|
||||
const float new_raw_z1 = startPosn.e + ratio.e * dist;
|
||||
if (cfg.linearAdvEna) {
|
||||
float dedt_adj = (new_raw_z1 - e_raw_z1) * (FTM_FS);
|
||||
float dedt_adj = (trajWin.e[makeVector_batchIdx] - e_raw_z1) * (FTM_FS);
|
||||
if (ratio.e > 0.0f) dedt_adj += accel_k * cfg.linearAdvK;
|
||||
|
||||
e_raw_z1 = trajWin.e[makeVector_batchIdx];
|
||||
e_advanced_z1 += dedt_adj * (FTM_TS);
|
||||
traj.e[makeVector_batchIdx] = e_advanced_z1;
|
||||
|
||||
e_raw_z1 = new_raw_z1;
|
||||
}
|
||||
else {
|
||||
traj.e[makeVector_batchIdx] = new_raw_z1;
|
||||
// Alternatively: ed[makeVector_batchIdx] = startPosn.e + (ratio.e * dist) / (N1 + N2 + N3);
|
||||
trajWin.e[makeVector_batchIdx] = e_advanced_z1;
|
||||
}
|
||||
#endif
|
||||
|
||||
// Update shaping parameters if needed.
|
||||
#if HAS_DYNAMIC_FREQ_MM
|
||||
static float zd_z1 = 0.0f;
|
||||
#endif
|
||||
|
||||
switch (cfg.dynFreqMode) {
|
||||
|
||||
#if HAS_DYNAMIC_FREQ_MM
|
||||
case dynFreqMode_Z_BASED:
|
||||
if (traj.z[makeVector_batchIdx] != zd_z1) { // Only update if Z changed.
|
||||
const float xf = cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * traj.z[makeVector_batchIdx],
|
||||
yf = cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * traj.z[makeVector_batchIdx];
|
||||
updateShapingN(_MAX(xf, FTM_MIN_SHAPE_FREQ), _MAX(yf, FTM_MIN_SHAPE_FREQ));
|
||||
zd_z1 = traj.z[makeVector_batchIdx];
|
||||
if (trajWin.z[makeVector_batchIdx] != 0.0f) { // Only update if Z changed.
|
||||
const float xf = cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * trajWin.z[makeVector_batchIdx]
|
||||
OPTARG(HAS_Y_AXIS, yf = cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * trajWin.z[makeVector_batchIdx]);
|
||||
updateShapingN(_MAX(xf, FTM_MIN_SHAPE_FREQ) OPTARG(HAS_Y_AXIS, _MAX(yf, FTM_MIN_SHAPE_FREQ)));
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
|
@ -610,8 +652,8 @@ void FTMotion::makeVector() {
|
|||
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[X_AXIS] + cfg.dynFreqK[X_AXIS] * traj.e[makeVector_batchIdx]
|
||||
OPTARG(HAS_Y_AXIS, cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * traj.e[makeVector_batchIdx]) );
|
||||
updateShapingN( cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * trajWin.e[makeVector_batchIdx]
|
||||
OPTARG(HAS_Y_AXIS, cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * trajWin.e[makeVector_batchIdx]) );
|
||||
break;
|
||||
#endif
|
||||
|
||||
|
|
@ -621,18 +663,18 @@ void FTMotion::makeVector() {
|
|||
// Apply shaping if in mode.
|
||||
#if HAS_X_AXIS
|
||||
if (cfg.modeHasShaper()) {
|
||||
shaping.x.d_zi[shaping.zi_idx] = traj.x[makeVector_batchIdx];
|
||||
traj.x[makeVector_batchIdx] *= shaping.x.Ai[0];
|
||||
shaping.x.d_zi[shaping.zi_idx] = trajWin.x[makeVector_batchIdx];
|
||||
trajWin.x[makeVector_batchIdx] *= shaping.x.Ai[0];
|
||||
#if HAS_Y_AXIS
|
||||
shaping.y.d_zi[shaping.zi_idx] = traj.y[makeVector_batchIdx];
|
||||
traj.y[makeVector_batchIdx] *= shaping.y.Ai[0];
|
||||
shaping.y.d_zi[shaping.zi_idx] = trajWin.y[makeVector_batchIdx];
|
||||
trajWin.y[makeVector_batchIdx] *= shaping.y.Ai[0];
|
||||
#endif
|
||||
for (uint32_t i = 1U; i <= shaping.max_i; i++) {
|
||||
const uint32_t udiffx = shaping.zi_idx - shaping.x.Ni[i];
|
||||
traj.x[makeVector_batchIdx] += shaping.x.Ai[i] * shaping.x.d_zi[shaping.x.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffx : udiffx];
|
||||
trajWin.x[makeVector_batchIdx] += shaping.x.Ai[i] * shaping.x.d_zi[shaping.x.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffx : udiffx];
|
||||
#if HAS_Y_AXIS
|
||||
const uint32_t udiffy = shaping.zi_idx - shaping.y.Ni[i];
|
||||
traj.y[makeVector_batchIdx] += shaping.y.Ai[i] * shaping.y.d_zi[shaping.y.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffy : udiffy];
|
||||
trajWin.y[makeVector_batchIdx] += shaping.y.Ai[i] * shaping.y.d_zi[shaping.y.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffy : udiffy];
|
||||
#endif
|
||||
}
|
||||
if (++shaping.zi_idx == (FTM_ZMAX)) shaping.zi_idx = 0;
|
||||
|
|
@ -640,18 +682,38 @@ void FTMotion::makeVector() {
|
|||
#endif
|
||||
|
||||
// Filled up the queue with regular and shaped steps
|
||||
if (++makeVector_batchIdx == (FTM_WINDOW_SIZE)) {
|
||||
makeVector_batchIdx = last_batchIdx;
|
||||
if (++makeVector_batchIdx == TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, (FTM_WINDOW_SIZE - FTM_BATCH_SIZE))) {
|
||||
makeVector_batchIdx = 0;
|
||||
batchRdy = true;
|
||||
}
|
||||
|
||||
if (makeVector_idx == max_intervals) {
|
||||
if (++makeVector_idx == max_intervals) {
|
||||
blockProcDn = true;
|
||||
blockProcRdy = false;
|
||||
makeVector_idx = 0;
|
||||
}
|
||||
else
|
||||
makeVector_idx++;
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert to steps
|
||||
* - Commands are written in a bitmask with step and dir as single bits.
|
||||
* - 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[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;
|
||||
s++;
|
||||
b |= bd | bs;
|
||||
e -= FTM_STEPS_PER_UNIT_TIME;
|
||||
}
|
||||
|
||||
static void command_set_neg(int32_t &e, int32_t &s, ft_command_t &b, int32_t bd, int32_t bs) {
|
||||
if (e > -(FTM_CTS_COMPARE_VAL)) return;
|
||||
s--;
|
||||
b |= bs;
|
||||
e += FTM_STEPS_PER_UNIT_TIME;
|
||||
}
|
||||
|
||||
// Interpolates single data point to stepper commands.
|
||||
|
|
@ -660,154 +722,61 @@ void FTMotion::convertToSteps(const uint32_t idx) {
|
|||
|
||||
//#define STEPS_ROUNDING
|
||||
#if ENABLED(STEPS_ROUNDING)
|
||||
const xyze_float_t steps_tar = LOGICAL_AXIS_ARRAY(
|
||||
trajMod.e[idx] * planner.settings.axis_steps_per_mm[E_AXIS_N(current_block->extruder)] + (trajMod.e[idx] < 0.0f ? -0.5f : 0.5f), // May be eliminated if guaranteed positive.
|
||||
trajMod.x[idx] * planner.settings.axis_steps_per_mm[X_AXIS] + (trajMod.x[idx] < 0.0f ? -0.5f : 0.5f),
|
||||
trajMod.y[idx] * planner.settings.axis_steps_per_mm[Y_AXIS] + (trajMod.y[idx] < 0.0f ? -0.5f : 0.5f),
|
||||
trajMod.z[idx] * planner.settings.axis_steps_per_mm[Z_AXIS] + (trajMod.z[idx] < 0.0f ? -0.5f : 0.5f),
|
||||
trajMod.i[idx] * planner.settings.axis_steps_per_mm[I_AXIS] + (trajMod.i[idx] < 0.0f ? -0.5f : 0.5f),
|
||||
trajMod.j[idx] * planner.settings.axis_steps_per_mm[J_AXIS] + (trajMod.j[idx] < 0.0f ? -0.5f : 0.5f),
|
||||
trajMod.k[idx] * planner.settings.axis_steps_per_mm[K_AXIS] + (trajMod.k[idx] < 0.0f ? -0.5f : 0.5f),
|
||||
trajMod.u[idx] * planner.settings.axis_steps_per_mm[U_AXIS] + (trajMod.u[idx] < 0.0f ? -0.5f : 0.5f),
|
||||
trajMod.v[idx] * planner.settings.axis_steps_per_mm[V_AXIS] + (trajMod.v[idx] < 0.0f ? -0.5f : 0.5f),
|
||||
trajMod.w[idx] * planner.settings.axis_steps_per_mm[W_AXIS] + (trajMod.w[idx] < 0.0f ? -0.5f : 0.5f),
|
||||
#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(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)
|
||||
);
|
||||
xyze_long_t delta = xyze_long_t(steps_tar) - steps;
|
||||
//const xyze_long_t delta = LOGICAL_AXIS_ARRAY(
|
||||
// int32_t(steps_tar.e) - steps.e,
|
||||
// int32_t(steps_tar.x) - steps.x,
|
||||
// int32_t(steps_tar.y) - steps.y,
|
||||
// int32_t(steps_tar.z) - steps.z,
|
||||
// int32_t(steps_tar.i) - steps.i,
|
||||
// int32_t(steps_tar.j) - steps.j,
|
||||
// int32_t(steps_tar.k) - steps.k,
|
||||
// int32_t(steps_tar.u) - steps.u,
|
||||
// int32_t(steps_tar.v) - steps.v,
|
||||
// int32_t(steps_tar.w) - steps.w
|
||||
//);
|
||||
xyze_long_t delta = steps_tar - steps;
|
||||
#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(
|
||||
int32_t(trajMod.e[idx] * planner.settings.axis_steps_per_mm[E_AXIS_N(current_block->extruder)]) - steps.e,
|
||||
int32_t(trajMod.x[idx] * planner.settings.axis_steps_per_mm[X_AXIS]) - steps.x,
|
||||
int32_t(trajMod.y[idx] * planner.settings.axis_steps_per_mm[Y_AXIS]) - steps.y,
|
||||
int32_t(trajMod.z[idx] * planner.settings.axis_steps_per_mm[Z_AXIS]) - steps.z,
|
||||
int32_t(trajMod.i[idx] * planner.settings.axis_steps_per_mm[I_AXIS]) - steps.i,
|
||||
int32_t(trajMod.j[idx] * planner.settings.axis_steps_per_mm[J_AXIS]) - steps.j,
|
||||
int32_t(trajMod.k[idx] * planner.settings.axis_steps_per_mm[K_AXIS]) - steps.k,
|
||||
int32_t(trajMod.u[idx] * planner.settings.axis_steps_per_mm[U_AXIS]) - steps.u,
|
||||
int32_t(trajMod.v[idx] * planner.settings.axis_steps_per_mm[V_AXIS]) - steps.v,
|
||||
int32_t(trajMod.w[idx] * planner.settings.axis_steps_per_mm[W_AXIS]) - steps.w
|
||||
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
|
||||
|
||||
bool any_dirChange = (false
|
||||
LOGICAL_AXIS_GANG(
|
||||
|| (delta.e > 0 && dirState.e != stepDirState_POS) || (delta.e < 0 && dirState.e != stepDirState_NEG),
|
||||
|| (delta.x > 0 && dirState.x != stepDirState_POS) || (delta.x < 0 && dirState.x != stepDirState_NEG),
|
||||
|| (delta.y > 0 && dirState.y != stepDirState_POS) || (delta.y < 0 && dirState.y != stepDirState_NEG),
|
||||
|| (delta.z > 0 && dirState.z != stepDirState_POS) || (delta.z < 0 && dirState.z != stepDirState_NEG),
|
||||
|| (delta.i > 0 && dirState.i != stepDirState_POS) || (delta.i < 0 && dirState.i != stepDirState_NEG),
|
||||
|| (delta.j > 0 && dirState.j != stepDirState_POS) || (delta.j < 0 && dirState.j != stepDirState_NEG),
|
||||
|| (delta.k > 0 && dirState.k != stepDirState_POS) || (delta.k < 0 && dirState.k != stepDirState_NEG),
|
||||
|| (delta.u > 0 && dirState.u != stepDirState_POS) || (delta.u < 0 && dirState.u != stepDirState_NEG),
|
||||
|| (delta.v > 0 && dirState.v != stepDirState_POS) || (delta.v < 0 && dirState.v != stepDirState_NEG),
|
||||
|| (delta.w > 0 && dirState.w != stepDirState_POS) || (delta.w < 0 && dirState.w != stepDirState_NEG)
|
||||
)
|
||||
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++) {
|
||||
|
||||
// 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;
|
||||
|
||||
// Commands are written in a bitmask with step and dir as single bits
|
||||
auto COMMAND_SET = [&](auto &d, auto &e, auto &s, auto &b, auto bd, auto bs) {
|
||||
if (d >= 0) {
|
||||
if (e + d < (FTM_CTS_COMPARE_VAL)) {
|
||||
e += d;
|
||||
}
|
||||
else {
|
||||
s++;
|
||||
b |= bd | bs;
|
||||
e += d - (FTM_STEPS_PER_UNIT_TIME);
|
||||
anyStep = true;
|
||||
}
|
||||
}
|
||||
else {
|
||||
if ((e + d) > -(FTM_CTS_COMPARE_VAL)) {
|
||||
e += d;
|
||||
}
|
||||
else {
|
||||
s--;
|
||||
b |= bs;
|
||||
e += d + (FTM_STEPS_PER_UNIT_TIME);
|
||||
anyStep = true;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
// Init all step/dir bits to 0 (defaulting to reverse/negative motion)
|
||||
stepperCmdBuff[stepperCmdBuff_produceIdx] = 0;
|
||||
|
||||
err_P += delta;
|
||||
|
||||
// Set up step/dir bits for all axes
|
||||
LOGICAL_AXIS_CODE(
|
||||
COMMAND_SET(delta.e, err_P.e, steps.e, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_E), _BV(FT_BIT_STEP_E)),
|
||||
COMMAND_SET(delta.x, err_P.x, steps.x, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_X), _BV(FT_BIT_STEP_X)),
|
||||
COMMAND_SET(delta.y, err_P.y, steps.y, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_Y), _BV(FT_BIT_STEP_Y)),
|
||||
COMMAND_SET(delta.z, err_P.z, steps.z, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_Z), _BV(FT_BIT_STEP_Z)),
|
||||
COMMAND_SET(delta.i, err_P.i, steps.i, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_I), _BV(FT_BIT_STEP_I)),
|
||||
COMMAND_SET(delta.j, err_P.j, steps.j, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_J), _BV(FT_BIT_STEP_J)),
|
||||
COMMAND_SET(delta.k, err_P.k, steps.k, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_K), _BV(FT_BIT_STEP_K)),
|
||||
COMMAND_SET(delta.u, err_P.u, steps.u, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_U), _BV(FT_BIT_STEP_U)),
|
||||
COMMAND_SET(delta.v, err_P.v, steps.v, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_V), _BV(FT_BIT_STEP_V)),
|
||||
COMMAND_SET(delta.w, err_P.w, steps.w, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_W), _BV(FT_BIT_STEP_W)),
|
||||
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)),
|
||||
);
|
||||
|
||||
if (!anyStep) {
|
||||
nextStepTicks += (FTM_MIN_TICKS);
|
||||
}
|
||||
else {
|
||||
stepperCmdBuff_StepRelativeTi[stepperCmdBuff_produceIdx] = nextStepTicks;
|
||||
if (++stepperCmdBuff_produceIdx == (FTM_STEPPERCMD_BUFF_SIZE))
|
||||
stepperCmdBuff_produceIdx = 0;
|
||||
|
||||
const uint8_t dir_index = stepperCmdBuff_produceIdx >> 3,
|
||||
dir_bit = stepperCmdBuff_produceIdx & 0x7;
|
||||
if (any_dirChange) {
|
||||
|
||||
SBI(stepperCmdBuff_ApplyDir[dir_index], dir_bit);
|
||||
|
||||
auto DIR_SET = [&](auto &d, auto &c, auto &b, auto bd) {
|
||||
if (d > 0) { b |= bd; c = stepDirState_POS; } else { c = stepDirState_NEG; }
|
||||
};
|
||||
|
||||
LOGICAL_AXIS_CODE(
|
||||
DIR_SET(delta.e, dirState.e, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_E)),
|
||||
DIR_SET(delta.x, dirState.x, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_X)),
|
||||
DIR_SET(delta.y, dirState.y, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_Y)),
|
||||
DIR_SET(delta.z, dirState.z, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_Z)),
|
||||
DIR_SET(delta.i, dirState.i, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_I)),
|
||||
DIR_SET(delta.j, dirState.j, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_J)),
|
||||
DIR_SET(delta.k, dirState.k, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_K)),
|
||||
DIR_SET(delta.u, dirState.u, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_U)),
|
||||
DIR_SET(delta.v, dirState.v, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_V)),
|
||||
DIR_SET(delta.w, dirState.w, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_W)),
|
||||
);
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -26,8 +26,6 @@
|
|||
|
||||
#include "ft_types.h"
|
||||
|
||||
#define FTM_STEPPERCMD_DIR_SIZE ((FTM_STEPPERCMD_BUFF_SIZE + 7) / 8)
|
||||
|
||||
#if HAS_X_AXIS && (HAS_Z_AXIS || HAS_EXTRUDERS)
|
||||
#define HAS_DYNAMIC_FREQ 1
|
||||
#if HAS_Z_AXIS
|
||||
|
|
@ -39,28 +37,30 @@
|
|||
#endif
|
||||
|
||||
typedef struct FTConfig {
|
||||
ftMotionMode_t mode = FTM_DEFAULT_MODE; // Mode / active compensation mode configuration.
|
||||
ftMotionMode_t mode = FTM_DEFAULT_MODE; // Mode / active compensation mode configuration.
|
||||
|
||||
bool modeHasShaper() { return WITHIN(mode, 10U, 19U); }
|
||||
|
||||
#if HAS_X_AXIS
|
||||
float baseFreq[1 + ENABLED(HAS_Y_AXIS)] = // Base frequency. [Hz]
|
||||
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) };
|
||||
float vtol[1 + ENABLED(HAS_Y_AXIS)] = // Vibration Level
|
||||
{ FTM_SHAPING_V_TOL_X OPTARG(HAS_Y_AXIS, FTM_SHAPING_V_TOL_Y) };
|
||||
#endif
|
||||
|
||||
float zeta = FTM_SHAPING_ZETA; // Damping factor
|
||||
float vtol = FTM_SHAPING_V_TOL; // Vibration Level
|
||||
|
||||
#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]
|
||||
#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;
|
||||
#endif
|
||||
|
||||
#if HAS_EXTRUDERS
|
||||
bool linearAdvEna = FTM_LINEAR_ADV_DEFAULT_ENA; // Linear advance enable configuration.
|
||||
float linearAdvK = FTM_LINEAR_ADV_DEFAULT_K; // Linear advance gain.
|
||||
bool linearAdvEna = FTM_LINEAR_ADV_DEFAULT_ENA; // Linear advance enable configuration.
|
||||
float linearAdvK = FTM_LINEAR_ADV_DEFAULT_K; // Linear advance gain.
|
||||
#endif
|
||||
} ft_config_t;
|
||||
|
||||
|
|
@ -78,8 +78,11 @@ class FTMotion {
|
|||
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);
|
||||
|
||||
cfg.zeta = FTM_SHAPING_ZETA; // Damping factor
|
||||
cfg.vtol = FTM_SHAPING_V_TOL; // Vibration Level
|
||||
TERN_(HAS_X_AXIS, cfg.zeta[X_AXIS] = FTM_SHAPING_ZETA_X);
|
||||
TERN_(HAS_Y_AXIS, cfg.zeta[Y_AXIS] = FTM_SHAPING_ZETA_Y);
|
||||
|
||||
TERN_(HAS_X_AXIS, cfg.vtol[X_AXIS] = FTM_SHAPING_V_TOL_X);
|
||||
TERN_(HAS_Y_AXIS, cfg.vtol[Y_AXIS] = FTM_SHAPING_V_TOL_Y);
|
||||
|
||||
#if HAS_DYNAMIC_FREQ
|
||||
cfg.dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE;
|
||||
|
|
@ -99,47 +102,45 @@ class FTMotion {
|
|||
reset();
|
||||
}
|
||||
|
||||
static ft_command_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 ft_command_t stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE]; // Buffer of stepper commands.
|
||||
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.
|
||||
static bool sts_stepperBusy; // The stepper buffer has items and is in use.
|
||||
|
||||
|
||||
// Public methods
|
||||
static void init();
|
||||
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.
|
||||
static void startBlockProc(); // 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=cfg.zeta, const_float_t vtol=cfg.vtol);
|
||||
static void updateShapingA(float zeta[]=cfg.zeta, float vtol[]=cfg.vtol);
|
||||
|
||||
// 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=cfg.zeta);
|
||||
static void updateShapingN(const_float_t xf OPTARG(HAS_Y_AXIS, const_float_t yf), float zeta[]=cfg.zeta);
|
||||
|
||||
static void refreshShapingN() { updateShapingN(cfg.baseFreq[X_AXIS] OPTARG(HAS_Y_AXIS, cfg.baseFreq[Y_AXIS])); }
|
||||
|
||||
#endif
|
||||
|
||||
static void reset(); // Resets all states of the fixed time conversion to defaults.
|
||||
static void reset(); // Reset all states of the fixed time conversion to defaults.
|
||||
|
||||
private:
|
||||
|
||||
static xyze_trajectory_t traj;
|
||||
static xyze_trajectoryMod_t trajMod;
|
||||
static xyze_trajectoryWin_t trajWin;
|
||||
|
||||
static block_t *current_block_cpy;
|
||||
static bool blockProcRdy, blockProcRdy_z1, blockProcDn;
|
||||
static bool batchRdy, batchRdyForInterp;
|
||||
static bool runoutEna;
|
||||
static bool runout;
|
||||
static bool blockDataIsRunout;
|
||||
|
||||
// Trapezoid data variables.
|
||||
static xyze_pos_t startPosn, // (mm) Start position of block
|
||||
|
|
@ -154,6 +155,11 @@ class FTMotion {
|
|||
static uint32_t N1, N2, N3;
|
||||
static uint32_t max_intervals;
|
||||
|
||||
static constexpr uint32_t _ftm_size = TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, FTM_BATCH_SIZE),
|
||||
_ftm_wind = TERN(FTM_UNIFIED_BWS, 2, ceil((FTM_WINDOW_SIZE) / _ftm_size)),
|
||||
shaper_intervals = _ftm_size * ceil((FTM_ZMAX) / _ftm_size),
|
||||
min_max_intervals = _ftm_size * _ftm_wind;
|
||||
|
||||
// Make vector variables.
|
||||
static uint32_t makeVector_idx,
|
||||
makeVector_idx_z1,
|
||||
|
|
@ -164,9 +170,6 @@ class FTMotion {
|
|||
interpIdx_z1;
|
||||
|
||||
static xyze_long_t steps;
|
||||
static xyze_stepDir_t dirState;
|
||||
|
||||
static hal_timer_t nextStepTicks;
|
||||
|
||||
// Shaping variables.
|
||||
#if HAS_X_AXIS
|
||||
|
|
@ -188,7 +191,7 @@ class FTMotion {
|
|||
axis_shaping_t y;
|
||||
#endif
|
||||
|
||||
void updateShapingA(const_float_t zeta=cfg.zeta, const_float_t vtol=cfg.vtol);
|
||||
void updateShapingA(float zeta[]=cfg.zeta, float vtol[]=cfg.vtol);
|
||||
|
||||
} shaping_t;
|
||||
|
||||
|
|
|
|||
|
|
@ -29,10 +29,12 @@ typedef enum FXDTICtrlMode : uint8_t {
|
|||
//ftMotionMode_ULENDO_FBS = 2U,
|
||||
ftMotionMode_ZV = 10U,
|
||||
ftMotionMode_ZVD = 11U,
|
||||
ftMotionMode_EI = 12U,
|
||||
ftMotionMode_2HEI = 13U,
|
||||
ftMotionMode_3HEI = 14U,
|
||||
ftMotionMode_MZV = 15U,
|
||||
ftMotionMode_ZVDD = 12U,
|
||||
ftMotionMode_ZVDDD = 13U,
|
||||
ftMotionMode_EI = 14U,
|
||||
ftMotionMode_2HEI = 15U,
|
||||
ftMotionMode_3HEI = 16U,
|
||||
ftMotionMode_MZV = 17U,
|
||||
//ftMotionMode_DISCTF = 20U
|
||||
} ftMotionMode_t;
|
||||
|
||||
|
|
@ -42,16 +44,17 @@ enum dynFreqMode_t : uint8_t {
|
|||
dynFreqMode_MASS_BASED = 2U
|
||||
};
|
||||
|
||||
enum stepDirState_t : uint8_t {
|
||||
stepDirState_NOT_SET = 0U,
|
||||
stepDirState_POS = 1U,
|
||||
stepDirState_NEG = 2U
|
||||
};
|
||||
#define IS_EI_MODE(N) WITHIN(N, ftMotionMode_EI, ftMotionMode_3HEI)
|
||||
|
||||
typedef struct XYZEarray<float, FTM_WINDOW_SIZE> xyze_trajectory_t;
|
||||
typedef struct XYZEarray<float, FTM_BATCH_SIZE> xyze_trajectoryMod_t;
|
||||
|
||||
typedef struct XYZEval<stepDirState_t> xyze_stepDir_t;
|
||||
#if ENABLED(FTM_UNIFIED_BWS)
|
||||
typedef struct XYZEarray<float, FTM_BW_SIZE> xyze_trajectory_t;
|
||||
typedef struct XYZEarray<float, FTM_BW_SIZE> xyze_trajectoryMod_t;
|
||||
typedef struct XYZEarray<float, FTM_BW_SIZE> xyze_trajectoryWin_t;
|
||||
#else
|
||||
typedef struct XYZEarray<float, FTM_WINDOW_SIZE> xyze_trajectory_t;
|
||||
typedef struct XYZEarray<float, FTM_BATCH_SIZE> xyze_trajectoryMod_t;
|
||||
typedef struct XYZEarray<float, (FTM_WINDOW_SIZE - FTM_BATCH_SIZE)> xyze_trajectoryWin_t;
|
||||
#endif
|
||||
|
||||
enum {
|
||||
LIST_N(DOUBLE(LOGICAL_AXES),
|
||||
|
|
|
|||
|
|
@ -33,10 +33,6 @@
|
|||
#include "../lcd/marlinui.h"
|
||||
#include "../inc/MarlinConfig.h"
|
||||
|
||||
#if ENABLED(FT_MOTION)
|
||||
#include "ft_motion.h"
|
||||
#endif
|
||||
|
||||
#if IS_SCARA
|
||||
#include "../libs/buzzer.h"
|
||||
#include "../lcd/marlinui.h"
|
||||
|
|
@ -2095,21 +2091,6 @@ void prepare_line_to_destination() {
|
|||
|
||||
void homeaxis(const AxisEnum axis) {
|
||||
|
||||
#if ENABLED(FT_MOTION)
|
||||
// Disable ft-motion for homing
|
||||
struct OnExit {
|
||||
ftMotionMode_t oldmm;
|
||||
OnExit() {
|
||||
oldmm = ftMotion.cfg.mode;
|
||||
ftMotion.cfg.mode = ftMotionMode_DISABLED;
|
||||
}
|
||||
~OnExit() {
|
||||
ftMotion.cfg.mode = oldmm;
|
||||
ftMotion.init();
|
||||
}
|
||||
} on_exit;
|
||||
#endif
|
||||
|
||||
#if ANY(MORGAN_SCARA, MP_SCARA)
|
||||
// Only Z homing (with probe) is permitted
|
||||
if (axis != Z_AXIS) { BUZZ(100, 880); return; }
|
||||
|
|
|
|||
|
|
@ -1492,11 +1492,6 @@ void Stepper::isr() {
|
|||
uint8_t max_loops = 10;
|
||||
|
||||
#if ENABLED(FT_MOTION)
|
||||
static bool ftMotion_stepCmdRdy = false; // Indicates a step command was loaded from the
|
||||
// buffers and is ready to be output.
|
||||
static bool ftMotion_applyDir = false; // Indicates the DIR output should be set.
|
||||
static ft_command_t ftMotion_stepCmd = 0U; // Storage for the step command to be output.
|
||||
static uint32_t ftMotion_nextAuxISR = 0U; // Storage for the next ISR of the auxilliary tasks.
|
||||
const bool using_ftMotion = ftMotion.cfg.mode;
|
||||
#else
|
||||
constexpr bool using_ftMotion = false;
|
||||
|
|
@ -1508,57 +1503,19 @@ void Stepper::isr() {
|
|||
// Enable ISRs to reduce USART processing latency
|
||||
hal.isr_on();
|
||||
|
||||
hal_timer_t interval;
|
||||
hal_timer_t interval = 0;
|
||||
|
||||
#if ENABLED(FT_MOTION)
|
||||
|
||||
if (using_ftMotion) {
|
||||
if (!nextMainISR) {
|
||||
if (abort_current_block) {
|
||||
ftMotion_stepCmdRdy = false; // If a command was ready, cancel it.
|
||||
ftMotion.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 (ftMotion_stepCmdRdy) {
|
||||
ftMotion_stepper(ftMotion_applyDir, ftMotion_stepCmd);
|
||||
ftMotion_stepCmdRdy = false;
|
||||
}
|
||||
// Check if there is data in the buffers.
|
||||
if (ftMotion.stepperCmdBuff_produceIdx != ftMotion.stepperCmdBuff_consumeIdx) {
|
||||
|
||||
ftMotion.sts_stepperBusy = true;
|
||||
|
||||
// "Pop" one command from the command buffer.
|
||||
ftMotion_stepCmd = ftMotion.stepperCmdBuff[ftMotion.stepperCmdBuff_consumeIdx];
|
||||
const uint8_t dir_index = ftMotion.stepperCmdBuff_consumeIdx >> 3,
|
||||
dir_bit = ftMotion.stepperCmdBuff_consumeIdx & 0x7;
|
||||
ftMotion_applyDir = TEST(ftMotion.stepperCmdBuff_ApplyDir[dir_index], dir_bit);
|
||||
nextMainISR = ftMotion.stepperCmdBuff_StepRelativeTi[ftMotion.stepperCmdBuff_consumeIdx];
|
||||
ftMotion_stepCmdRdy = true;
|
||||
|
||||
if (++ftMotion.stepperCmdBuff_consumeIdx == (FTM_STEPPERCMD_BUFF_SIZE))
|
||||
ftMotion.stepperCmdBuff_consumeIdx = 0;
|
||||
|
||||
}
|
||||
else { // Buffer empty.
|
||||
ftMotion.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 auxiliary functions.
|
||||
if (!ftMotion_nextAuxISR) {
|
||||
nextMainISR = FTM_MIN_TICKS;
|
||||
ftMotion_stepper();
|
||||
endstops.update();
|
||||
TERN_(BABYSTEPPING, if (babystep.has_steps()) babystepping_isr());
|
||||
ftMotion_refreshAxisDidMove();
|
||||
ftMotion_nextAuxISR = 0.0025f * (STEPPER_TIMER_RATE);
|
||||
}
|
||||
|
||||
interval = _MIN(nextMainISR, ftMotion_nextAuxISR);
|
||||
interval = nextMainISR;
|
||||
nextMainISR -= interval;
|
||||
ftMotion_nextAuxISR -= interval;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -3365,6 +3322,28 @@ void Stepper::set_axis_position(const AxisEnum a, const int32_t &v) {
|
|||
#endif
|
||||
}
|
||||
|
||||
#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
|
||||
count_position = planner.position;
|
||||
|
||||
#ifdef __AVR__
|
||||
// Reenable Stepper ISR
|
||||
if (was_enabled) wake_up();
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif // FT_MOTION
|
||||
|
||||
// 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
|
||||
|
|
@ -3459,65 +3438,70 @@ void Stepper::report_positions() {
|
|||
#if ENABLED(FT_MOTION)
|
||||
|
||||
// Set stepper I/O for fixed time controller.
|
||||
void Stepper::ftMotion_stepper(const bool applyDir, const ft_command_t command) {
|
||||
void Stepper::ftMotion_stepper() {
|
||||
|
||||
// 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 = 0U;
|
||||
|
||||
if (abort_current_block) return;
|
||||
|
||||
USING_TIMED_PULSE();
|
||||
|
||||
const xyze_bool_t axis_step = LOGICAL_AXIS_ARRAY(
|
||||
axis_did_move = 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)
|
||||
);
|
||||
|
||||
last_direction_bits = LOGICAL_AXIS_ARRAY(
|
||||
axis_did_move.e ? TEST(command, FT_BIT_DIR_E) : last_direction_bits.e,
|
||||
axis_did_move.x ? TEST(command, FT_BIT_DIR_X) : last_direction_bits.x,
|
||||
axis_did_move.y ? TEST(command, FT_BIT_DIR_Y) : last_direction_bits.y,
|
||||
axis_did_move.z ? TEST(command, FT_BIT_DIR_Z) : last_direction_bits.z,
|
||||
axis_did_move.i ? TEST(command, FT_BIT_DIR_I) : last_direction_bits.i,
|
||||
axis_did_move.j ? TEST(command, FT_BIT_DIR_J) : last_direction_bits.j,
|
||||
axis_did_move.k ? TEST(command, FT_BIT_DIR_K) : last_direction_bits.k,
|
||||
axis_did_move.u ? TEST(command, FT_BIT_DIR_U) : last_direction_bits.u,
|
||||
axis_did_move.v ? TEST(command, FT_BIT_DIR_V) : last_direction_bits.v,
|
||||
axis_did_move.w ? TEST(command, FT_BIT_DIR_W) : last_direction_bits.w
|
||||
);
|
||||
|
||||
// Apply directions (which will apply to the entire linear move)
|
||||
AxisBits axis_dir = last_direction_bits;
|
||||
if (applyDir) {
|
||||
axis_dir = LOGICAL_AXIS_ARRAY(
|
||||
TEST(command, FT_BIT_DIR_E),
|
||||
TEST(command, FT_BIT_DIR_X), TEST(command, FT_BIT_DIR_Y), TEST(command, FT_BIT_DIR_Z),
|
||||
TEST(command, FT_BIT_DIR_I), TEST(command, FT_BIT_DIR_J), TEST(command, FT_BIT_DIR_K),
|
||||
TEST(command, FT_BIT_DIR_U), TEST(command, FT_BIT_DIR_V), TEST(command, FT_BIT_DIR_W)
|
||||
);
|
||||
LOGICAL_AXIS_CODE(
|
||||
E_APPLY_DIR(axis_dir.e, false),
|
||||
X_APPLY_DIR(axis_dir.x, false), Y_APPLY_DIR(axis_dir.y, false), Z_APPLY_DIR(axis_dir.z, false),
|
||||
I_APPLY_DIR(axis_dir.i, false), J_APPLY_DIR(axis_dir.j, false), K_APPLY_DIR(axis_dir.k, false),
|
||||
U_APPLY_DIR(axis_dir.u, false), V_APPLY_DIR(axis_dir.v, false), W_APPLY_DIR(axis_dir.w, false)
|
||||
);
|
||||
last_direction_bits = axis_dir;
|
||||
DIR_WAIT_AFTER();
|
||||
}
|
||||
LOGICAL_AXIS_CODE(
|
||||
E_APPLY_DIR(last_direction_bits.e, false),
|
||||
X_APPLY_DIR(last_direction_bits.x, false), Y_APPLY_DIR(last_direction_bits.y, false), Z_APPLY_DIR(last_direction_bits.z, false),
|
||||
I_APPLY_DIR(last_direction_bits.i, false), J_APPLY_DIR(last_direction_bits.j, false), K_APPLY_DIR(last_direction_bits.k, false),
|
||||
U_APPLY_DIR(last_direction_bits.u, false), V_APPLY_DIR(last_direction_bits.v, false), W_APPLY_DIR(last_direction_bits.w, false)
|
||||
);
|
||||
|
||||
DIR_WAIT_AFTER();
|
||||
|
||||
// Start a step pulse
|
||||
LOGICAL_AXIS_CODE(
|
||||
if (axis_step.e) E_APPLY_STEP(STEP_STATE_E, false),
|
||||
if (axis_step.x) X_APPLY_STEP(STEP_STATE_X, false), if (axis_step.y) Y_APPLY_STEP(STEP_STATE_Y, false),
|
||||
if (axis_step.z) Z_APPLY_STEP(STEP_STATE_Z, false), if (axis_step.i) I_APPLY_STEP(STEP_STATE_I, false),
|
||||
if (axis_step.j) J_APPLY_STEP(STEP_STATE_J, false), if (axis_step.k) K_APPLY_STEP(STEP_STATE_K, false),
|
||||
if (axis_step.u) U_APPLY_STEP(STEP_STATE_U, false), if (axis_step.v) V_APPLY_STEP(STEP_STATE_V, false),
|
||||
if (axis_step.w) W_APPLY_STEP(STEP_STATE_W, false)
|
||||
E_APPLY_STEP(axis_did_move.e, false),
|
||||
X_APPLY_STEP(axis_did_move.x, false), Y_APPLY_STEP(axis_did_move.y, false), Z_APPLY_STEP(axis_did_move.z, false),
|
||||
I_APPLY_STEP(axis_did_move.i, false), J_APPLY_STEP(axis_did_move.j, false), K_APPLY_STEP(axis_did_move.k, false),
|
||||
U_APPLY_STEP(axis_did_move.u, false), V_APPLY_STEP(axis_did_move.v, false), W_APPLY_STEP(axis_did_move.w, false)
|
||||
);
|
||||
|
||||
// Begin waiting for the minimum pulse duration
|
||||
START_TIMED_PULSE();
|
||||
|
||||
// Update axis direction adders
|
||||
count_direction = LOGICAL_AXIS_ARRAY(
|
||||
int8_t(axis_dir.e ? 1 : -1),
|
||||
int8_t(axis_dir.x ? 1 : -1), int8_t(axis_dir.y ? 1 : -1), int8_t(axis_dir.z ? 1 : -1),
|
||||
int8_t(axis_dir.i ? 1 : -1), int8_t(axis_dir.j ? 1 : -1), int8_t(axis_dir.k ? 1 : -1),
|
||||
int8_t(axis_dir.u ? 1 : -1), int8_t(axis_dir.v ? 1 : -1), int8_t(axis_dir.w ? 1 : -1)
|
||||
);
|
||||
|
||||
// Update stepper counts - required for various operations
|
||||
// Update step counts
|
||||
LOGICAL_AXIS_CODE(
|
||||
if (axis_step.e) count_position.e += count_direction.e,
|
||||
if (axis_step.x) count_position.x += count_direction.x, if (axis_step.y) count_position.y += count_direction.y,
|
||||
if (axis_step.z) count_position.z += count_direction.z, if (axis_step.i) count_position.i += count_direction.i,
|
||||
if (axis_step.j) count_position.j += count_direction.j, if (axis_step.k) count_position.k += count_direction.k,
|
||||
if (axis_step.u) count_position.u += count_direction.u, if (axis_step.v) count_position.v += count_direction.v,
|
||||
if (axis_step.w) count_position.w += count_direction.w
|
||||
if (axis_did_move.e) count_position.e += last_direction_bits.e ? 1 : -1, if (axis_did_move.x) count_position.x += last_direction_bits.x ? 1 : -1,
|
||||
if (axis_did_move.y) count_position.y += last_direction_bits.y ? 1 : -1, if (axis_did_move.z) count_position.z += last_direction_bits.z ? 1 : -1,
|
||||
if (axis_did_move.i) count_position.i += last_direction_bits.i ? 1 : -1, if (axis_did_move.j) count_position.j += last_direction_bits.j ? 1 : -1,
|
||||
if (axis_did_move.k) count_position.k += last_direction_bits.k ? 1 : -1, if (axis_did_move.u) count_position.u += last_direction_bits.u ? 1 : -1,
|
||||
if (axis_did_move.v) count_position.v += last_direction_bits.v ? 1 : -1, if (axis_did_move.w) count_position.w += last_direction_bits.w ? 1 : -1
|
||||
);
|
||||
|
||||
#if HAS_EXTRUDERS
|
||||
|
|
@ -3532,10 +3516,10 @@ void Stepper::report_positions() {
|
|||
|
||||
// Only wait for axes without edge stepping
|
||||
const bool any_wait = false LOGICAL_AXIS_GANG(
|
||||
|| (!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)
|
||||
|| (!e_axis_has_dedge && axis_did_move.e),
|
||||
|| (!AXIS_HAS_DEDGE(X) && axis_did_move.x), || (!AXIS_HAS_DEDGE(Y) && axis_did_move.y), || (!AXIS_HAS_DEDGE(Z) && axis_did_move.z),
|
||||
|| (!AXIS_HAS_DEDGE(I) && axis_did_move.i), || (!AXIS_HAS_DEDGE(J) && axis_did_move.j), || (!AXIS_HAS_DEDGE(K) && axis_did_move.k),
|
||||
|| (!AXIS_HAS_DEDGE(U) && axis_did_move.u), || (!AXIS_HAS_DEDGE(V) && axis_did_move.v), || (!AXIS_HAS_DEDGE(W) && axis_did_move.w)
|
||||
);
|
||||
|
||||
// Allow pulses to be registered by stepper drivers
|
||||
|
|
@ -3543,80 +3527,46 @@ void Stepper::report_positions() {
|
|||
|
||||
// Stop pulses. Axes with DEDGE will do nothing, assuming STEP_STATE_* is HIGH
|
||||
LOGICAL_AXIS_CODE(
|
||||
if (axis_step.e) E_APPLY_STEP(!STEP_STATE_E, false),
|
||||
if (axis_step.x) X_APPLY_STEP(!STEP_STATE_X, false), if (axis_step.y) Y_APPLY_STEP(!STEP_STATE_Y, false),
|
||||
if (axis_step.z) Z_APPLY_STEP(!STEP_STATE_Z, false), if (axis_step.i) I_APPLY_STEP(!STEP_STATE_I, false),
|
||||
if (axis_step.j) J_APPLY_STEP(!STEP_STATE_J, false), if (axis_step.k) K_APPLY_STEP(!STEP_STATE_K, false),
|
||||
if (axis_step.u) U_APPLY_STEP(!STEP_STATE_U, false), if (axis_step.v) V_APPLY_STEP(!STEP_STATE_V, false),
|
||||
if (axis_step.w) W_APPLY_STEP(!STEP_STATE_W, false)
|
||||
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
|
||||
|
||||
void Stepper::ftMotion_BlockQueueUpdate() {
|
||||
void Stepper::ftMotion_blockQueueUpdate() {
|
||||
|
||||
if (current_block) {
|
||||
// If the current block is not done processing, return right away
|
||||
if (!ftMotion.getBlockProcDn()) return;
|
||||
|
||||
axis_did_move.reset();
|
||||
current_block = nullptr;
|
||||
discard_current_block();
|
||||
planner.release_current_block();
|
||||
}
|
||||
|
||||
if (!current_block) { // No current block
|
||||
// Check the buffer for a new block
|
||||
current_block = planner.get_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()) {
|
||||
TERN_(LASER_FEATURE, if (!(current_block->is_fan_sync() || current_block->is_pwr_sync()))) _set_position(current_block->position);
|
||||
|
||||
planner.release_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;
|
||||
|
||||
ftMotion.startBlockProc(current_block);
|
||||
|
||||
}
|
||||
else {
|
||||
ftMotion.runoutBlock();
|
||||
return; // No queued blocks
|
||||
// Try to get a new block
|
||||
if (!(current_block = planner.get_current_block()))
|
||||
return; // No queued blocks.
|
||||
}
|
||||
|
||||
} // if (!current_block)
|
||||
ftMotion.startBlockProc();
|
||||
return;
|
||||
}
|
||||
|
||||
} // Stepper::ftMotion_BlockQueueUpdate()
|
||||
ftMotion.runoutBlock();
|
||||
|
||||
// Debounces the axis move indication to account for potential
|
||||
// delay between the block information and the stepper commands
|
||||
void Stepper::ftMotion_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.
|
||||
|
||||
AxisBits didmove;
|
||||
static abce_ulong_t debounce{0};
|
||||
auto debounce_axis = [&](const AxisEnum axis) {
|
||||
if (current_block->steps[axis]) debounce[axis] = (AXIS_DID_MOVE_DEB) * 400; // divide by 0.0025f */
|
||||
if (debounce[axis]) { didmove.bset(axis); debounce[axis]--; }
|
||||
};
|
||||
#define _DEBOUNCE(N) debounce_axis(AxisEnum(N));
|
||||
|
||||
if (current_block) { REPEAT(LOGICAL_AXES, _DEBOUNCE); }
|
||||
|
||||
axis_did_move = didmove;
|
||||
}
|
||||
} // Stepper::ftMotion_blockQueueUpdate()
|
||||
|
||||
#endif // FT_MOTION
|
||||
|
||||
|
|
|
|||
|
|
@ -655,7 +655,9 @@ class Stepper {
|
|||
|
||||
#if ENABLED(FT_MOTION)
|
||||
// Manage the planner
|
||||
static void ftMotion_BlockQueueUpdate();
|
||||
static void ftMotion_blockQueueUpdate();
|
||||
// Set current position in steps when reset flag is set in M493 and planner already synchronized
|
||||
static void ftMotion_syncPosition();
|
||||
#endif
|
||||
|
||||
#if HAS_ZV_SHAPING
|
||||
|
|
@ -694,8 +696,7 @@ class Stepper {
|
|||
#endif
|
||||
|
||||
#if ENABLED(FT_MOTION)
|
||||
static void ftMotion_stepper(const bool applyDir, const ft_command_t command);
|
||||
static void ftMotion_refreshAxisDidMove();
|
||||
static void ftMotion_stepper();
|
||||
#endif
|
||||
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in a new issue