From 7b23fd3548f5aabc269ba24ec947d6fdfd225072 Mon Sep 17 00:00:00 2001 From: narno2202 <130909513+narno2202@users.noreply.github.com> Date: Fri, 30 Jun 2023 00:06:05 +0200 Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9B=20Fix=20FT=20Motion=20runout=20in?= =?UTF-8?q?=20progress=20(#26020)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/Configuration_adv.h | 2 +- Marlin/src/module/ft_motion.cpp | 39 ++++++++++++++++++++++++++++----- Marlin/src/module/ft_motion.h | 1 + Marlin/src/module/ft_types.h | 2 +- 4 files changed, 36 insertions(+), 8 deletions(-) diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index eee111a5412a..be54354d5c0c 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1125,7 +1125,7 @@ * Advanced configuration */ #define FTM_BATCH_SIZE 100 // Batch size for trajectory generation; - // half the window size for Ulendo FBS. + #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. diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index d3c4de896254..6fe8e936e0e5 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -75,6 +75,7 @@ bool FxdTiCtrl::batchRdy = false; // Indicates a batch of the fi bool FxdTiCtrl::batchRdyForInterp = false; // Indicates the batch is done being post processed, // if applicable, and is ready to be converted to step commands. bool FxdTiCtrl::runoutEna = false; // True if runout of the block hasn't been done and is allowed. +bool FxdTiCtrl::runout = false; // Indicates if runout is in progress. // Trapezoid data variables. xyze_pos_t FxdTiCtrl::startPosn, // (mm) Start position of block @@ -123,6 +124,8 @@ hal_timer_t FxdTiCtrl::nextStepTicks = FTM_MIN_TICKS; // Accumulator for the nex float FxdTiCtrl::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. //-----------------------------------------------------------------// @@ -143,8 +146,8 @@ void FxdTiCtrl::runoutBlock() { if (runoutEna && !batchRdy) { // If the window is full already (block intervals was a multiple of // the batch size), or runout is not enabled, no runout is needed. // Fill out the trajectory window with the last position calculated. - if (makeVector_batchIdx > FTM_BATCH_SIZE) - for (uint32_t i = makeVector_batchIdx; i < 2 * (FTM_BATCH_SIZE); i++) { + 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], @@ -159,8 +162,9 @@ void FxdTiCtrl::runoutBlock() { ); } - makeVector_batchIdx = FTM_BATCH_SIZE; + makeVector_batchIdx = last_batchIdx; batchRdy = true; + runout = true; } runoutEna = false; } @@ -184,7 +188,7 @@ void FxdTiCtrl::loop() { } // Planner processing and block conversion. - if (!blockProcRdy) stepper.fxdTiCtrl_BlockQueueUpdate(); + if (!blockProcRdy && !runout) stepper.fxdTiCtrl_BlockQueueUpdate(); if (blockProcRdy) { if (!blockProcRdy_z1) loadBlockData(current_block_cpy); // One-shot. @@ -192,6 +196,27 @@ void FxdTiCtrl::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) { @@ -371,10 +396,12 @@ void FxdTiCtrl::reset() { stepperCmdBuff_produceIdx = stepperCmdBuff_consumeIdx = 0; traj.reset(); // Reset trajectory history + trajMod.reset(); // Reset modified trajectory history blockProcRdy = blockProcRdy_z1 = blockProcDn = false; batchRdy = batchRdyForInterp = false; runoutEna = false; + runout = false; endPosn_prevBlock.reset(); @@ -611,8 +638,8 @@ void FxdTiCtrl::makeVector() { #endif // Filled up the queue with regular and shaped steps - if (++makeVector_batchIdx == 2 * (FTM_BATCH_SIZE)) { - makeVector_batchIdx = FTM_BATCH_SIZE; + if (++makeVector_batchIdx == (FTM_WINDOW_SIZE)) { + makeVector_batchIdx = last_batchIdx; batchRdy = true; } diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h index f4be977786fe..2186ecb710a8 100644 --- a/Marlin/src/module/ft_motion.h +++ b/Marlin/src/module/ft_motion.h @@ -133,6 +133,7 @@ class FxdTiCtrl { static bool blockProcRdy, blockProcRdy_z1, blockProcDn; static bool batchRdy, batchRdyForInterp; static bool runoutEna; + static bool runout; // Trapezoid data variables. static xyze_pos_t startPosn, // (mm) Start position of block diff --git a/Marlin/src/module/ft_types.h b/Marlin/src/module/ft_types.h index c4465bd37bb8..a7228d32d6f2 100644 --- a/Marlin/src/module/ft_types.h +++ b/Marlin/src/module/ft_types.h @@ -48,7 +48,7 @@ enum stepDirState_t : uint8_t { stepDirState_NEG = 2U }; -typedef struct XYZEarray xyze_trajectory_t; +typedef struct XYZEarray xyze_trajectory_t; typedef struct XYZEarray xyze_trajectoryMod_t; typedef struct XYZEval xyze_stepDir_t;