diff --git a/Marlin/src/core/types.h b/Marlin/src/core/types.h index 85fd517b68..bbb152174f 100644 --- a/Marlin/src/core/types.h +++ b/Marlin/src/core/types.h @@ -46,6 +46,7 @@ template struct IF { typedef L type; }; #define NUM_AXIS_ELEM(O) NUM_AXIS_LIST(O.x, O.y, O.z, O.i, O.j, O.k, O.u, O.v, O.w) #define NUM_AXIS_DECL(T,V) NUM_AXIS_LIST(T x=V, T y=V, T z=V, T i=V, T j=V, T k=V, T u=V, T v=V, T w=V) #define MAIN_AXIS_NAMES NUM_AXIS_LIST(X, Y, Z, I, J, K, U, V, W) +#define MAIN_AXIS_NAMES_LC NUM_AXIS_LIST(x, y, z, i, j, k, u, v, w) #define STR_AXES_MAIN NUM_AXIS_GANG("X", "Y", "Z", STR_I, STR_J, STR_K, STR_U, STR_V, STR_W) #define LOGICAL_AXIS_GANG(E,V...) NUM_AXIS_GANG(V) GANG_ITEM_E(E) @@ -58,17 +59,21 @@ template struct IF { typedef L type; }; #define LOGICAL_AXIS_ELEM(O) LOGICAL_AXIS_LIST(O.e, O.x, O.y, O.z, O.i, O.j, O.k, O.u, O.v, O.w) #define LOGICAL_AXIS_DECL(T,V) LOGICAL_AXIS_LIST(T e=V, T x=V, T y=V, T z=V, T i=V, T j=V, T k=V, T u=V, T v=V, T w=V) #define LOGICAL_AXIS_NAMES LOGICAL_AXIS_LIST(E, X, Y, Z, I, J, K, U, V, W) +#define LOGICAL_AXIS_NAMES_LC LOGICAL_AXIS_LIST(e, x, y, z, i, j, k, u, v, w) #define LOGICAL_AXIS_MAP(F) MAP(F, LOGICAL_AXIS_NAMES) +#define LOGICAL_AXIS_MAP_LC(F) MAP(F, LOGICAL_AXIS_NAMES_LC) #define STR_AXES_LOGICAL LOGICAL_AXIS_GANG("E", "X", "Y", "Z", STR_I, STR_J, STR_K, STR_U, STR_V, STR_W) #if NUM_AXES #define NUM_AXES_SEP , #define MAIN_AXIS_MAP(F) MAP(F, MAIN_AXIS_NAMES) + #define MAIN_AXIS_MAP_LC(F) MAP(F, MAIN_AXIS_NAMES_LC) #define OPTARGS_NUM(T) , NUM_AXIS_ARGS(T) #define OPTARGS_LOGICAL(T) , LOGICAL_AXIS_ARGS(T) #else #define NUM_AXES_SEP #define MAIN_AXIS_MAP(F) + #define MAIN_AXIS_MAP_LC(F) #define OPTARGS_NUM(T) #define OPTARGS_LOGICAL(T) #endif @@ -79,6 +84,7 @@ template struct IF { typedef L type; }; #define NUM_AXIS_ARGS_(T) NUM_AXIS_ARGS(T) NUM_AXES_SEP #define NUM_AXIS_ELEM_(T) NUM_AXIS_ELEM(T) NUM_AXES_SEP #define MAIN_AXIS_NAMES_ MAIN_AXIS_NAMES NUM_AXES_SEP +#define MAIN_AXIS_NAMES_LC_ MAIN_AXIS_NAMES_LC NUM_AXES_SEP #if LOGICAL_AXES #define LOGICAL_AXES_SEP , @@ -92,6 +98,7 @@ template struct IF { typedef L type; }; #define LOGICAL_AXIS_ARGS_(T) LOGICAL_AXIS_ARGS(T) LOGICAL_AXES_SEP #define LOGICAL_AXIS_ELEM_(T) LOGICAL_AXIS_ELEM(T) LOGICAL_AXES_SEP #define LOGICAL_AXIS_NAMES_ LOGICAL_AXIS_NAMES LOGICAL_AXES_SEP +#define LOGICAL_AXIS_NAMES_LC_ LOGICAL_AXIS_NAMES_LC LOGICAL_AXES_SEP #define SECONDARY_AXIS_GANG(V...) GANG_N(SECONDARY_AXES, V) #define SECONDARY_AXIS_CODE(V...) CODE_N(SECONDARY_AXES, V) @@ -219,7 +226,7 @@ typedef struct { // // - X_AXIS, Y_AXIS, and Z_AXIS should be used for axes in Cartesian space // - A_AXIS, B_AXIS, and C_AXIS should be used for Steppers, corresponding to XYZ on Cartesians -// - X_HEAD, Y_HEAD, and Z_HEAD should be used for Steppers on Core kinematics +// - X_HEAD, Y_HEAD, and Z_HEAD should be used for axes on Core kinematics // enum AxisEnum : uint8_t { @@ -1084,6 +1091,7 @@ public: FI bool toggle(const AxisEnum n) { TBI(bits, n); return TEST(bits, n); } FI void bset(const AxisEnum n) { SBI(bits, n); } FI void bclr(const AxisEnum n) { CBI(bits, n); } + FI void bset(const AxisEnum n, const bool b) { if (b) bset(n); else bclr(n); } // Accessor via an AxisEnum (or any integer) [index] FI bool operator[](const int n) const { return TEST(bits, n); } diff --git a/Marlin/src/inc/Conditionals_LCD.h b/Marlin/src/inc/Conditionals_LCD.h index 2a8c02f912..efda2d493e 100644 --- a/Marlin/src/inc/Conditionals_LCD.h +++ b/Marlin/src/inc/Conditionals_LCD.h @@ -1603,8 +1603,6 @@ #endif #if CORE_IS_XY || CORE_IS_XZ || CORE_IS_YZ #define IS_CORE 1 -#endif -#if IS_CORE #if CORE_IS_XY #define CORE_AXIS_1 A_AXIS #define CORE_AXIS_2 B_AXIS diff --git a/Marlin/src/inc/Conditionals_adv.h b/Marlin/src/inc/Conditionals_adv.h index 78da97f67b..de3d8d2b24 100644 --- a/Marlin/src/inc/Conditionals_adv.h +++ b/Marlin/src/inc/Conditionals_adv.h @@ -1185,7 +1185,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 70 + #define MINIMUM_STEPPER_POST_DIR_DELAY 100 #else #define MINIMUM_STEPPER_POST_DIR_DELAY 0 // Expect at least 10µS since one Stepper ISR must transpire #endif diff --git a/Marlin/src/module/endstops.cpp b/Marlin/src/module/endstops.cpp index 09154b65a2..d0ad234a7f 100644 --- a/Marlin/src/module/endstops.cpp +++ b/Marlin/src/module/endstops.cpp @@ -50,6 +50,10 @@ #include "../feature/joystick.h" #endif +#if ENABLED(FT_MOTION) + #include "ft_motion.h" +#endif + #if HAS_BED_PROBE #include "probe.h" #endif @@ -782,6 +786,7 @@ void Endstops::update() { #define PROCESS_ENDSTOP_Z(MINMAX) PROCESS_DUAL_ENDSTOP(Z, MINMAX) #endif + #if ENABLED(G38_PROBE_TARGET) // For G38 moves check the probe's pin for ALL movement if (G38_move && TEST_ENDSTOP(Z_MIN_PROBE) == TERN1(G38_PROBE_AWAY, (G38_move < 4))) { @@ -796,8 +801,17 @@ void Endstops::update() { // Signal, after validation, if an endstop limit is pressed or not #if HAS_X_AXIS - if (stepper.axis_is_moving(X_AXIS)) { - if (!stepper.motor_direction(X_AXIS_HEAD)) { // -direction + #if ENABLED(FT_MOTION) + const bool x_moving_pos = ftMotion.axis_moving_pos(X_AXIS_HEAD), + x_moving_neg = ftMotion.axis_moving_neg(X_AXIS_HEAD); + #define X_MOVE_TEST x_moving_pos || x_moving_neg + #define X_NEG_DIR_TEST x_moving_neg + #else + #define X_MOVE_TEST stepper.axis_is_moving(X_AXIS) + #define X_NEG_DIR_TEST !stepper.motor_direction(X_AXIS_HEAD) + #endif + if (X_MOVE_TEST) { + if (X_NEG_DIR_TEST) { // -direction #if HAS_X_MIN_STATE PROCESS_ENDSTOP_X(MIN); #if CORE_DIAG(XY, Y, MIN) @@ -829,8 +843,17 @@ void Endstops::update() { #endif // HAS_X_AXIS #if HAS_Y_AXIS - if (stepper.axis_is_moving(Y_AXIS)) { - if (!stepper.motor_direction(Y_AXIS_HEAD)) { // -direction + #if ENABLED(FT_MOTION) + const bool y_moving_pos = ftMotion.axis_moving_pos(Y_AXIS_HEAD), + y_moving_neg = ftMotion.axis_moving_neg(Y_AXIS_HEAD); + #define Y_MOVE_TEST y_moving_pos || y_moving_neg + #define Y_NEG_DIR_TEST y_moving_neg + #else + #define Y_MOVE_TEST stepper.axis_is_moving(Y_AXIS) + #define Y_NEG_DIR_TEST !stepper.motor_direction(Y_AXIS_HEAD) + #endif + if (Y_MOVE_TEST) { + if (Y_NEG_DIR_TEST) { // -direction #if HAS_Y_MIN_STATE PROCESS_ENDSTOP_Y(MIN); #if CORE_DIAG(XY, X, MIN) @@ -862,8 +885,17 @@ void Endstops::update() { #endif // HAS_Y_AXIS #if HAS_Z_AXIS - if (stepper.axis_is_moving(Z_AXIS)) { - if (!stepper.motor_direction(Z_AXIS_HEAD)) { // Z -direction. Gantry down, bed up. + #if ENABLED(FT_MOTION) + const bool z_moving_pos = ftMotion.axis_moving_pos(Z_AXIS_HEAD), + z_moving_neg = ftMotion.axis_moving_neg(Z_AXIS_HEAD); + #define Z_MOVE_TEST z_moving_pos || z_moving_neg + #define Z_NEG_DIR_TEST z_moving_neg + #else + #define Z_MOVE_TEST stepper.axis_is_moving(Z_AXIS) + #define Z_NEG_DIR_TEST !stepper.motor_direction(Z_AXIS_HEAD) + #endif + if (Z_MOVE_TEST) { + if (Z_NEG_DIR_TEST) { // Z -direction. Gantry down, bed up. #if HAS_Z_MIN_STATE // If the Z_MIN_PIN is being used for the probe there's no // separate Z_MIN endstop. But a Z endstop could be wired @@ -907,6 +939,7 @@ void Endstops::update() { #endif // HAS_Z_AXIS #if HAS_I_AXIS + // TODO: FT_Motion logic. if (stepper.axis_is_moving(I_AXIS)) { if (!stepper.motor_direction(I_AXIS_HEAD)) { // -direction #if HAS_I_MIN_STATE diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index e912255561..f92caa7d72 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -26,6 +26,7 @@ #include "ft_motion.h" #include "stepper.h" // Access stepper block queue function and abort status. +#include "endstops.h" FTMotion ftMotion; @@ -59,6 +60,9 @@ int32_t FTMotion::stepperCmdBuff_produceIdx = 0, // Index of next stepper comman FTMotion::stepperCmdBuff_consumeIdx = 0; // Index of next stepper command read from the buffer. bool FTMotion::sts_stepperBusy = false; // The stepper buffer has items and is in use. +millis_t FTMotion::axis_pos_move_end_ti[NUM_AXIS_ENUMS] = {0}, + FTMotion::axis_neg_move_end_ti[NUM_AXIS_ENUMS] = {0}; + // Private variables. @@ -110,9 +114,9 @@ uint32_t FTMotion::interpIdx = 0, // Index of current data point b #if HAS_X_AXIS FTMotion::shaping_t FTMotion::shaping = { 0, 0, - x:{ { 0.0f }, { 0.0f }, { 0 } }, // d_zi, Ai, Ni + x:{ false, { 0.0f }, { 0.0f }, { 0 } }, // d_zi, Ai, Ni #if HAS_Y_AXIS - y:{ { 0.0f }, { 0.0f }, { 0 } } // d_zi, Ai, Ni + y:{ false, { 0.0f }, { 0.0f }, { 0 } } // d_zi, Ai, Ni #endif }; #endif @@ -131,7 +135,10 @@ constexpr uint32_t last_batchIdx = (FTM_WINDOW_SIZE) - (FTM_BATCH_SIZE); // Public functions. +static bool markBlockStart = false; + // Sets controller states to begin processing a block. +// Called by Stepper::ftMotion_blockQueueUpdate, invoked from the main loop. void FTMotion::startBlockProc() { blockProcRdy = true; blockProcDn = false; @@ -166,11 +173,13 @@ void FTMotion::loop() { if (!cfg.mode) return; - // Handle block abort with the following sequence: - // 1. Zero out commands in stepper ISR. - // 2. Drain the motion buffer, stop processing until they are emptied. - // 3. Reset all the states / memory. - // 4. Signal ready for new block. + /** + * Handle block abort with the following sequence: + * 1. Zero out commands in stepper ISR. + * 2. Drain the motion buffer, stop processing until they are emptied. + * 3. Reset all the states / memory. + * 4. Signal ready for new block. + */ if (stepper.abort_current_block) { if (sts_stepperBusy) return; // Wait until motion buffers are emptied reset(); @@ -183,7 +192,10 @@ void FTMotion::loop() { if (blockProcRdy) { if (!blockProcRdy_z1) { // One-shot. - if (!blockDataIsRunout) loadBlockData(stepper.current_block); + if (!blockDataIsRunout) { + loadBlockData(stepper.current_block); + markBlockStart = true; + } else blockDataIsRunout = false; } while (!blockProcDn && !batchRdy && (makeVector_idx - makeVector_idx_z1 < (FTM_POINTS_PER_LOOP))) @@ -199,22 +211,12 @@ void FTMotion::loop() { trajMod = traj; // 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) - ); + #define TCOPY(A) memcpy(trajMod.A, traj.A, sizeof(trajMod.A)); + LOGICAL_AXIS_MAP_LC(TCOPY); // Shift the time series back in the window - #define TSHIFT(A) memcpy(traj.A, &traj.A[FTM_BATCH_SIZE], last_batchIdx * sizeof(traj.A[0])) - LOGICAL_AXIS_CODE( - TSHIFT(e), - TSHIFT(x), TSHIFT(y), TSHIFT(z), - TSHIFT(i), TSHIFT(j), TSHIFT(k), - TSHIFT(u), TSHIFT(v), TSHIFT(w) - ); + #define TSHIFT(A) memcpy(traj.A, &traj.A[FTM_BATCH_SIZE], last_batchIdx * sizeof(traj.A[0])); + LOGICAL_AXIS_MAP_LC(TSHIFT); #endif // ... data is ready in trajMod. @@ -471,6 +473,9 @@ void FTMotion::reset() { #endif TERN_(HAS_EXTRUDERS, e_raw_z1 = e_advanced_z1 = 0.0f); + + ZERO(axis_pos_move_end_ti); + ZERO(axis_neg_move_end_ti); } // Private functions. @@ -490,14 +495,14 @@ void FTMotion::init() { reset(); // Precautionary. } -// Loads / converts block data from planner to fixed-time control variables. +// Load / convert block data from planner to fixed-time control variables. void FTMotion::loadBlockData(block_t * const current_block) { const float totalLength = current_block->millimeters, oneOverLength = 1.0f / totalLength; startPosn = endPosn_prevBlock; - xyze_pos_t moveDist = LOGICAL_AXIS_ARRAY( + const xyze_pos_t moveDist = LOGICAL_AXIS_ARRAY( current_block->steps.e * planner.mm_per_step[E_AXIS_N(current_block->extruder)] * (current_block->direction_bits.e ? 1 : -1), current_block->steps.x * planner.mm_per_step[X_AXIS] * (current_block->direction_bits.x ? 1 : -1), current_block->steps.y * planner.mm_per_step[Y_AXIS] * (current_block->direction_bits.y ? 1 : -1), @@ -574,7 +579,8 @@ void FTMotion::loadBlockData(block_t * const current_block) { * f_s * T1_P : (mm) Distance traveled during the accel phase * f_e * T3_P : (mm) Distance traveled during the decel phase */ - 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 + const float adist = f_s * T1_P; + F_P = (2.0f * totalLength - adist - f_e * T3_P) / (T1_P + 2.0f * T2_P + T3_P); // (mm/s) Feedrate at the end of the accel phase // Calculate the acceleration and deceleration rates accel_P = N1 ? ((F_P - f_s) / T1_P) : 0.0f; @@ -582,7 +588,7 @@ void FTMotion::loadBlockData(block_t * const current_block) { decel_P = (f_e - F_P) / T3_P; // Calculate the distance traveled during the accel phase - s_1e = f_s * T1_P + 0.5f * accel_P * sq(T1_P); + s_1e = adist + 0.5f * accel_P * sq(T1_P); // Calculate the distance traveled during the decel phase s_2e = s_1e + F_P * T2_P; @@ -591,6 +597,43 @@ void FTMotion::loadBlockData(block_t * const current_block) { max_intervals = N1 + N2 + N3; endPosn_prevBlock += moveDist; + + millis_t move_end_ti = millis() + SEC_TO_MS(FTM_TS*(float)(max_intervals + num_samples_cmpnstr_settle() + (PROP_BATCHES+1)*FTM_BATCH_SIZE) + ((float)FTM_STEPPERCMD_BUFF_SIZE/(float)FTM_STEPPER_FS)); + + #if CORE_IS_XY + if (moveDist.x > 0.f) axis_pos_move_end_ti[A_AXIS] = move_end_ti; + if (moveDist.y > 0.f) axis_pos_move_end_ti[B_AXIS] = move_end_ti; + if (moveDist.x + moveDist.y > 0.f) axis_pos_move_end_ti[X_HEAD] = move_end_ti; + if (moveDist.x - moveDist.y > 0.f) axis_pos_move_end_ti[Y_HEAD] = move_end_ti; + if (moveDist.x < 0.f) axis_neg_move_end_ti[A_AXIS] = move_end_ti; + if (moveDist.y < 0.f) axis_neg_move_end_ti[B_AXIS] = move_end_ti; + if (moveDist.x + moveDist.y < 0.f) axis_neg_move_end_ti[X_HEAD] = move_end_ti; + if (moveDist.x - moveDist.y < 0.f) axis_neg_move_end_ti[Y_HEAD] = move_end_ti; + #else + if (moveDist.x > 0.f) axis_pos_move_end_ti[X_AXIS] = move_end_ti; + if (moveDist.y > 0.f) axis_pos_move_end_ti[Y_AXIS] = move_end_ti; + if (moveDist.x < 0.f) axis_neg_move_end_ti[X_AXIS] = move_end_ti; + if (moveDist.y < 0.f) axis_neg_move_end_ti[Y_AXIS] = move_end_ti; + #endif + if (moveDist.z > 0.f) axis_pos_move_end_ti[Z_AXIS] = move_end_ti; + if (moveDist.z < 0.f) axis_neg_move_end_ti[Z_AXIS] = move_end_ti; + // if (moveDist.i > 0.f) axis_pos_move_end_ti[I_AXIS] = move_end_ti; + // if (moveDist.i < 0.f) axis_neg_move_end_ti[I_AXIS] = move_end_ti; + // if (moveDist.j > 0.f) axis_pos_move_end_ti[J_AXIS] = move_end_ti; + // if (moveDist.j < 0.f) axis_neg_move_end_ti[J_AXIS] = move_end_ti; + // if (moveDist.k > 0.f) axis_pos_move_end_ti[K_AXIS] = move_end_ti; + // if (moveDist.k < 0.f) axis_neg_move_end_ti[K_AXIS] = move_end_ti; + // if (moveDist.u > 0.f) axis_pos_move_end_ti[U_AXIS] = move_end_ti; + // if (moveDist.u < 0.f) axis_neg_move_end_ti[U_AXIS] = move_end_ti; + // . + // . + // . + + // If the endstop is already pressed, endstop interrupts won't invoke + // endstop_triggered and the move will grind. So check here for a + // triggered endstop, which shortly marks the block for discard. + endstops.update(); + } // Generate data points of the trajectory. @@ -607,7 +650,6 @@ void FTMotion::makeVector() { else if (makeVector_idx < (N1 + N2)) { // Coasting phase dist = s_1e + F_P * (tau - N1 * (FTM_TS)); // (mm) Distance traveled for coasting phase since start of block - //accel_k = 0.0f; } else { // Deceleration phase @@ -616,18 +658,8 @@ void FTMotion::makeVector() { accel_k = decel_P; // (mm/s^2) Acceleration K factor from Decel phase } - LOGICAL_AXIS_CODE( - traj.e[makeVector_batchIdx] = startPosn.e + ratio.e * dist, - traj.x[makeVector_batchIdx] = startPosn.x + ratio.x * dist, - traj.y[makeVector_batchIdx] = startPosn.y + ratio.y * dist, - traj.z[makeVector_batchIdx] = startPosn.z + ratio.z * dist, - traj.i[makeVector_batchIdx] = startPosn.i + ratio.i * dist, - traj.j[makeVector_batchIdx] = startPosn.j + ratio.j * dist, - traj.k[makeVector_batchIdx] = startPosn.k + ratio.k * dist, - traj.u[makeVector_batchIdx] = startPosn.u + ratio.u * dist, - traj.v[makeVector_batchIdx] = startPosn.v + ratio.v * dist, - traj.w[makeVector_batchIdx] = startPosn.w + ratio.w * dist - ); + #define _FTM_TRAJ(A) traj.A[makeVector_batchIdx] = startPosn.A + ratio.A * dist; + LOGICAL_AXIS_MAP_LC(_FTM_TRAJ); #if HAS_EXTRUDERS if (cfg.linearAdvEna) { @@ -706,7 +738,7 @@ void FTMotion::makeVector() { * - Tests for delta are moved outside the loop. * - Two functions are used for command computation with an array of function pointers. */ -static void (*command_set[NUM_AXES TERN0(HAS_EXTRUDERS, +1)])(int32_t&, int32_t&, ft_command_t&, int32_t, int32_t); +static void (*command_set[SUM_TERN(HAS_EXTRUDERS, NUM_AXES, 1)])(int32_t&, int32_t&, ft_command_t&, int32_t, int32_t); static void command_set_pos(int32_t &e, int32_t &s, ft_command_t &b, int32_t bd, int32_t bs) { if (e < FTM_CTS_COMPARE_VAL) return; @@ -746,40 +778,30 @@ void FTMotion::convertToSteps(const uint32_t idx) { ); #endif - 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 - ); + #define _COMMAND_SET(AXIS) command_set[_AXIS(AXIS)] = delta[_AXIS(AXIS)] >= 0 ? command_set_pos : command_set_neg; + LOGICAL_AXIS_MAP(_COMMAND_SET); for (uint32_t i = 0U; i < (FTM_STEPS_PER_UNIT_TIME); i++) { - // Init all step/dir bits to 0 (defaulting to reverse/negative motion) - stepperCmdBuff[stepperCmdBuff_produceIdx] = 0; + ft_command_t &cmd = stepperCmdBuff[stepperCmdBuff_produceIdx]; + // Init all step/dir bits to 0 (defaulting to reverse/negative motion) + cmd = 0; + + // Mark the start of a new block + if (markBlockStart) { + cmd = _BV(FT_BIT_START); + markBlockStart = false; + } + + // Accumulate the errors for all axes err_P += delta; // Set up step/dir bits for all axes - LOGICAL_AXIS_CODE( - command_set[E_AXIS_N(current_block->extruder)](err_P.e, steps.e, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_E), _BV(FT_BIT_STEP_E)), - command_set[X_AXIS](err_P.x, steps.x, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_X), _BV(FT_BIT_STEP_X)), - command_set[Y_AXIS](err_P.y, steps.y, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_Y), _BV(FT_BIT_STEP_Y)), - command_set[Z_AXIS](err_P.z, steps.z, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_Z), _BV(FT_BIT_STEP_Z)), - command_set[I_AXIS](err_P.i, steps.i, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_I), _BV(FT_BIT_STEP_I)), - command_set[J_AXIS](err_P.j, steps.j, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_J), _BV(FT_BIT_STEP_J)), - command_set[K_AXIS](err_P.k, steps.k, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_K), _BV(FT_BIT_STEP_K)), - command_set[U_AXIS](err_P.u, steps.u, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_U), _BV(FT_BIT_STEP_U)), - command_set[V_AXIS](err_P.v, steps.v, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_V), _BV(FT_BIT_STEP_V)), - command_set[W_AXIS](err_P.w, steps.w, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_W), _BV(FT_BIT_STEP_W)), - ); + #define _COMMAND_RUN(AXIS) command_set[_AXIS(AXIS)](err_P[_AXIS(AXIS)], steps[_AXIS(AXIS)], cmd, _BV(FT_BIT_DIR_##AXIS), _BV(FT_BIT_STEP_##AXIS)); + LOGICAL_AXIS_MAP(_COMMAND_RUN); + // Next circular buffer index if (++stepperCmdBuff_produceIdx == (FTM_STEPPERCMD_BUFF_SIZE)) stepperCmdBuff_produceIdx = 0; diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h index 884a183479..3325005ae2 100644 --- a/Marlin/src/module/ft_motion.h +++ b/Marlin/src/module/ft_motion.h @@ -107,6 +107,9 @@ class FTMotion { static bool sts_stepperBusy; // The stepper buffer has items and is in use. + static millis_t axis_pos_move_end_ti[NUM_AXIS_ENUMS], + axis_neg_move_end_ti[NUM_AXIS_ENUMS]; + // Public methods static void init(); static void startBlockProc(); // Set controller states to begin processing a block. @@ -129,6 +132,9 @@ class FTMotion { static void reset(); // Reset all states of the fixed time conversion to defaults. + static bool axis_moving_pos(const AxisEnum axis) { return !ELAPSED(millis(), axis_pos_move_end_ti[axis]); } + static bool axis_moving_neg(const AxisEnum axis) { return !ELAPSED(millis(), axis_neg_move_end_ti[axis]); } + private: static xyze_trajectory_t traj; @@ -152,6 +158,8 @@ class FTMotion { static uint32_t N1, N2, N3; static uint32_t max_intervals; + static constexpr uint32_t PROP_BATCHES = CEIL(FTM_WINDOW_SIZE/FTM_BATCH_SIZE) - 1; // Number of batches needed to propagate the current trajectory to the stepper. + #define _DIVCEIL(A,B) (((A) + (B) - 1) / (B)) static constexpr uint32_t _ftm_ratio = TERN(FTM_UNIFIED_BWS, 2, _DIVCEIL(FTM_WINDOW_SIZE, FTM_BATCH_SIZE)), shaper_intervals = (FTM_BATCH_SIZE) * _DIVCEIL(FTM_ZMAX, FTM_BATCH_SIZE), @@ -172,6 +180,7 @@ class FTMotion { #if HAS_X_AXIS typedef struct AxisShaping { + bool ena = false; // Enabled indication. float d_zi[FTM_ZMAX] = { 0.0f }; // Data point delay vector. float Ai[5]; // Shaping gain vector. uint32_t Ni[5]; // Shaping time index vector. @@ -207,6 +216,9 @@ class FTMotion { static void makeVector(); static void convertToSteps(const uint32_t idx); + FORCE_INLINE static int32_t num_samples_cmpnstr_settle() { return ( shaping.x.ena || shaping.y.ena ) ? FTM_ZMAX : 0; } + + }; // class FTMotion extern FTMotion ftMotion; diff --git a/Marlin/src/module/ft_types.h b/Marlin/src/module/ft_types.h index d460853262..e6a2506017 100644 --- a/Marlin/src/module/ft_types.h +++ b/Marlin/src/module/ft_types.h @@ -47,7 +47,9 @@ enum dynFreqMode_t : uint8_t { typedef struct XYZEarray xyze_trajectory_t; typedef struct XYZEarray xyze_trajectoryMod_t; +// TODO: Convert ft_command_t to a struct with bitfields instead of using a primitive type enum { + FT_BIT_START, LIST_N(DOUBLE(LOGICAL_AXES), FT_BIT_DIR_E, FT_BIT_STEP_E, FT_BIT_DIR_X, FT_BIT_STEP_X, FT_BIT_DIR_Y, FT_BIT_STEP_Y, FT_BIT_DIR_Z, FT_BIT_STEP_Z, diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index c21230f662..37c344bfe7 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -1973,32 +1973,35 @@ bool Planner::_populate_block( // Compute direction bit-mask for this block AxisBits dm; - #if ANY(CORE_IS_XY, MARKFORGED_XY, MARKFORGED_YX) - dm.hx = (dist.a > 0); // Save the toolhead's true direction in X - dm.hy = (dist.b > 0); // ...and Y - TERN_(HAS_Z_AXIS, dm.z = (dist.c > 0)); + #if ANY(CORE_IS_XY, CORE_IS_XZ, MARKFORGED_XY, MARKFORGED_YX) + dm.hx = (dist.a > 0); // True direction in X + #endif + #if ANY(CORE_IS_XY, CORE_IS_YZ, MARKFORGED_XY, MARKFORGED_YX) + dm.hy = (dist.b > 0); // True direction in Y + #endif + #if ANY(CORE_IS_XZ, CORE_IS_YZ) + dm.hz = (dist.c > 0); // True direction in Z #endif #if CORE_IS_XY - dm.a = (dist.a + dist.b > 0); // Motor A direction - dm.b = (CORESIGN(dist.a - dist.b) > 0); // Motor B direction + dm.a = (dist.a + dist.b > 0); // Motor A direction + dm.b = (CORESIGN(dist.a - dist.b) > 0); // Motor B direction + TERN_(HAS_Z_AXIS, dm.z = (dist.c > 0)); // Axis Z direction #elif CORE_IS_XZ - dm.hx = (dist.a > 0); // Save the toolhead's true direction in X - dm.y = (dist.b > 0); - dm.hz = (dist.c > 0); // ...and Z dm.a = (dist.a + dist.c > 0); // Motor A direction + dm.y = (dist.b > 0); // Axis Y direction dm.c = (CORESIGN(dist.a - dist.c) > 0); // Motor C direction #elif CORE_IS_YZ - dm.x = (dist.a > 0); - dm.hy = (dist.b > 0); // Save the toolhead's true direction in Y - dm.hz = (dist.c > 0); // ...and Z + dm.x = (dist.a > 0); // Axis X direction dm.b = (dist.b + dist.c > 0); // Motor B direction dm.c = (CORESIGN(dist.b - dist.c) > 0); // Motor C direction #elif ENABLED(MARKFORGED_XY) dm.a = (dist.a TERN(MARKFORGED_INVERSE, -, +) dist.b > 0); // Motor A direction dm.b = (dist.b > 0); // Motor B direction + TERN_(HAS_Z_AXIS, dm.z = (dist.c > 0)); // Axis Z direction #elif ENABLED(MARKFORGED_YX) dm.a = (dist.a > 0); // Motor A direction dm.b = (dist.b TERN(MARKFORGED_INVERSE, -, +) dist.a > 0); // Motor B direction + TERN_(HAS_Z_AXIS, dm.z = (dist.c > 0)); // Axis Z direction #else XYZ_CODE( dm.x = (dist.a > 0), @@ -2895,10 +2898,12 @@ void Planner::buffer_sync_block(const BlockFlagBit sync_flag/*=BLOCK_BIT_SYNC_PO block->flag.apply(sync_flag); block->position = position; + #if ENABLED(BACKLASH_COMPENSATION) LOOP_NUM_AXES(axis) block->position[axis] += backlash.get_applied_steps((AxisEnum)axis); #endif - #if ALL(HAS_FAN, LASER_SYNCHRONOUS_M106_M107) + + #if ENABLED(LASER_SYNCHRONOUS_M106_M107) FANS_LOOP(i) block->fan_speed[i] = thermalManager.fan_speed[i]; #endif @@ -3227,6 +3232,10 @@ bool Planner::buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s * The provided ABCE position is in machine units. */ void Planner::set_machine_position_mm(const abce_pos_t &abce) { + + // When FT Motion is enabled, call synchronize() here instead of generating a sync block + if (TERN0(FT_MOTION, ftMotion.cfg.mode)) synchronize(); + TERN_(DISTINCT_E_FACTORS, last_extruder = active_extruder); TERN_(HAS_POSITION_FLOAT, position_float = abce); position.set( diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index b7b1abbb61..e783b0cd0f 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -214,9 +214,10 @@ typedef struct PlannerBlock { volatile block_flags_t flag; // Block flags - bool is_fan_sync() { return TERN0(LASER_SYNCHRONOUS_M106_M107, flag.sync_fans); } - bool is_pwr_sync() { return TERN0(LASER_POWER_SYNC, flag.sync_laser_pwr); } - bool is_sync() { return flag.sync_position || is_fan_sync() || is_pwr_sync(); } + bool is_sync_pos() { return flag.sync_position; } + bool is_sync_fan() { return TERN0(LASER_SYNCHRONOUS_M106_M107, flag.sync_fans); } + bool is_sync_pwr() { return TERN0(LASER_POWER_SYNC, flag.sync_laser_pwr); } + bool is_sync() { return is_sync_pos() || is_sync_fan() || is_sync_pwr(); } bool is_page() { return TERN0(DIRECT_STEPPING, flag.page); } bool is_move() { return !(is_sync() || is_page()); } diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 6426c7f4a2..252bf5526b 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -647,6 +647,11 @@ void Stepper::disable_all_steppers() { TERN_(EXTENSIBLE_UI, ExtUI::onSteppersDisabled()); } +#if ENABLED(FTM_OPTIMIZE_DIR_STATES) + // We'll compare the updated DIR bits to the last set state + static AxisBits last_set_direction; +#endif + // Set a single axis direction based on the last set flags. // A direction bit of "1" indicates forward or positive motion. #define SET_STEP_DIR(A) do{ \ @@ -672,6 +677,8 @@ void Stepper::apply_directions() { SET_STEP_DIR(U), SET_STEP_DIR(V), SET_STEP_DIR(W) ); + TERN_(FTM_OPTIMIZE_DIR_STATES, last_set_direction = last_direction_bits); + DIR_WAIT_AFTER(); } @@ -1542,8 +1549,20 @@ void Stepper::isr() { nextMainISR = FTM_MIN_TICKS; // Set to minimum interval (a limit on the top speed) ftMotion_stepper(); // Run FTM Stepping } - interval = nextMainISR; // Interval is either some old nextMainISR or FTM_MIN_TICKS - nextMainISR = 0; // For FT Motion fire again ASAP + + #if ENABLED(BABYSTEPPING) + if (nextBabystepISR == 0) { // Avoid ANY stepping too soon after baby-stepping + nextBabystepISR = babystepping_isr(); + NOLESS(nextMainISR, (BABYSTEP_TICKS) / 8); // FULL STOP for 125µs after a baby-step + } + if (nextBabystepISR != BABYSTEP_NEVER) // Avoid baby-stepping too close to axis Stepping + NOLESS(nextBabystepISR, nextMainISR / 2); // TODO: Only look at axes enabled for baby-stepping + #endif + + interval = nextMainISR; // Interval is either some old nextMainISR or FTM_MIN_TICKS + TERN_(BABYSTEPPING, NOMORE(interval, nextBabystepISR)); // Come back early for Babystepping? + + nextMainISR = 0; // For FT Motion fire again ASAP } #endif @@ -1801,6 +1820,7 @@ void Stepper::pulse_phase_isr() { last_direction_bits.toggle(_AXIS(AXIS)); \ DIR_WAIT_BEFORE(); \ SET_STEP_DIR(AXIS); \ + TERN_(FTM_OPTIMIZE_DIR_STATES, last_set_direction = last_direction_bits); \ DIR_WAIT_AFTER(); \ } \ } \ @@ -2248,6 +2268,90 @@ hal_timer_t Stepper::calc_multistep_timer_interval(uint32_t step_rate) { return calc_timer_interval(step_rate); } +// Method to get all moving axes (for proper endstop handling) +void Stepper::set_axis_moved_for_current_block() { + + #if IS_CORE + // Define conditions for checking endstops + #define S_(N) current_block->steps[CORE_AXIS_##N] + #define D_(N) current_block->direction_bits[CORE_AXIS_##N] + #endif + + #if CORE_IS_XY || CORE_IS_XZ + /** + * Head direction in -X axis for CoreXY and CoreXZ bots. + * + * If steps differ, both axes are moving. + * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z, handled below) + * If DeltaA == DeltaB, the movement is only in the 1st axis (X) + */ + #if ANY(COREXY, COREXZ) + #define X_CMP(A,B) ((A)==(B)) + #else + #define X_CMP(A,B) ((A)!=(B)) + #endif + #define X_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && X_CMP(D_(1),D_(2))) ) + #elif ENABLED(MARKFORGED_XY) + #define X_MOVE_TEST (current_block->steps.a != current_block->steps.b) + #else + #define X_MOVE_TEST !!current_block->steps.a + #endif + + #if CORE_IS_XY || CORE_IS_YZ + /** + * Head direction in -Y axis for CoreXY / CoreYZ bots. + * + * If steps differ, both axes are moving + * If DeltaA == DeltaB, the movement is only in the 1st axis (X or Y) + * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z) + */ + #if ANY(COREYX, COREYZ) + #define Y_CMP(A,B) ((A)==(B)) + #else + #define Y_CMP(A,B) ((A)!=(B)) + #endif + #define Y_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && Y_CMP(D_(1),D_(2))) ) + #elif ENABLED(MARKFORGED_YX) + #define Y_MOVE_TEST (current_block->steps.a != current_block->steps.b) + #else + #define Y_MOVE_TEST !!current_block->steps.b + #endif + + #if CORE_IS_XZ || CORE_IS_YZ + /** + * Head direction in -Z axis for CoreXZ or CoreYZ bots. + * + * If steps differ, both axes are moving + * If DeltaA == DeltaB, the movement is only in the 1st axis (X or Y, already handled above) + * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Z) + */ + #if ANY(COREZX, COREZY) + #define Z_CMP(A,B) ((A)==(B)) + #else + #define Z_CMP(A,B) ((A)!=(B)) + #endif + #define Z_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && Z_CMP(D_(1),D_(2))) ) + #else + #define Z_MOVE_TEST !!current_block->steps.c + #endif + + // Set flags for all axes that move in this block + // These are set per-axis, not per-stepper + AxisBits didmove; + NUM_AXIS_CODE( + if (X_MOVE_TEST) didmove.a = true, // Cartesian X or Kinematic A + if (Y_MOVE_TEST) didmove.b = true, // Cartesian Y or Kinematic B + if (Z_MOVE_TEST) didmove.c = true, // Cartesian Z or Kinematic C + if (!!current_block->steps.i) didmove.i = true, + if (!!current_block->steps.j) didmove.j = true, + if (!!current_block->steps.k) didmove.k = true, + if (!!current_block->steps.u) didmove.u = true, + if (!!current_block->steps.v) didmove.v = true, + if (!!current_block->steps.w) didmove.w = true + ); + axis_did_move = didmove; +} + /** * This last phase of the stepper interrupt processes and properly * schedules planner blocks. This is executed after the step pulses @@ -2410,6 +2514,8 @@ hal_timer_t Stepper::block_phase_isr() { E_APPLY_DIR(forward_e, false); + TERN_(FTM_OPTIMIZE_DIR_STATES, last_set_direction = last_direction_bits); + DIR_WAIT_AFTER(); } } @@ -2508,25 +2614,31 @@ hal_timer_t Stepper::block_phase_isr() { // Anything in the buffer? if ((current_block = planner.get_current_block())) { - // Sync block? Sync the stepper counts or fan speeds and return + // Run through all sync blocks while (current_block->is_sync()) { + // Set laser power #if ENABLED(LASER_POWER_SYNC) if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { - if (current_block->is_pwr_sync()) { + if (current_block->is_sync_pwr()) { planner.laser_inline.status.isSyncPower = true; cutter.apply_power(current_block->laser.power); } } #endif - TERN_(LASER_SYNCHRONOUS_M106_M107, if (current_block->is_fan_sync()) planner.sync_fan_speeds(current_block->fan_speed)); + // Set "fan speeds" for a laser module + #if ENABLED(LASER_SYNCHRONOUS_M106_M107) + if (current_block->is_sync_fan()) planner.sync_fan_speeds(current_block->fan_speed); + #endif - if (!(current_block->is_fan_sync() || current_block->is_pwr_sync())) _set_position(current_block->position); + // Set position + if (current_block->is_sync_pos()) _set_position(current_block->position); + // Done with this block discard_current_block(); - // Try to get a new block + // Try to get a new block. Exit if there are no more. if (!(current_block = planner.get_current_block())) return interval; // No more queued movements! } @@ -2560,85 +2672,8 @@ hal_timer_t Stepper::block_phase_isr() { } #endif - // Flag all moving axes for proper endstop handling - - #if IS_CORE - // Define conditions for checking endstops - #define S_(N) current_block->steps[CORE_AXIS_##N] - #define D_(N) current_block->direction_bits[CORE_AXIS_##N] - #endif - - #if CORE_IS_XY || CORE_IS_XZ - /** - * Head direction in -X axis for CoreXY and CoreXZ bots. - * - * If steps differ, both axes are moving. - * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z, handled below) - * If DeltaA == DeltaB, the movement is only in the 1st axis (X) - */ - #if ANY(COREXY, COREXZ) - #define X_CMP(A,B) ((A)==(B)) - #else - #define X_CMP(A,B) ((A)!=(B)) - #endif - #define X_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && X_CMP(D_(1),D_(2))) ) - #elif ENABLED(MARKFORGED_XY) - #define X_MOVE_TEST (current_block->steps.a != current_block->steps.b) - #else - #define X_MOVE_TEST !!current_block->steps.a - #endif - - #if CORE_IS_XY || CORE_IS_YZ - /** - * Head direction in -Y axis for CoreXY / CoreYZ bots. - * - * If steps differ, both axes are moving - * If DeltaA == DeltaB, the movement is only in the 1st axis (X or Y) - * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z) - */ - #if ANY(COREYX, COREYZ) - #define Y_CMP(A,B) ((A)==(B)) - #else - #define Y_CMP(A,B) ((A)!=(B)) - #endif - #define Y_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && Y_CMP(D_(1),D_(2))) ) - #elif ENABLED(MARKFORGED_YX) - #define Y_MOVE_TEST (current_block->steps.a != current_block->steps.b) - #else - #define Y_MOVE_TEST !!current_block->steps.b - #endif - - #if CORE_IS_XZ || CORE_IS_YZ - /** - * Head direction in -Z axis for CoreXZ or CoreYZ bots. - * - * If steps differ, both axes are moving - * If DeltaA == DeltaB, the movement is only in the 1st axis (X or Y, already handled above) - * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Z) - */ - #if ANY(COREZX, COREZY) - #define Z_CMP(A,B) ((A)==(B)) - #else - #define Z_CMP(A,B) ((A)!=(B)) - #endif - #define Z_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && Z_CMP(D_(1),D_(2))) ) - #else - #define Z_MOVE_TEST !!current_block->steps.c - #endif - - AxisBits didmove; - NUM_AXIS_CODE( - if (X_MOVE_TEST) didmove.a = true, - if (Y_MOVE_TEST) didmove.b = true, - if (Z_MOVE_TEST) didmove.c = true, - if (!!current_block->steps.i) didmove.i = true, - if (!!current_block->steps.j) didmove.j = true, - if (!!current_block->steps.k) didmove.k = true, - if (!!current_block->steps.u) didmove.u = true, - if (!!current_block->steps.v) didmove.v = true, - if (!!current_block->steps.w) didmove.w = true - ); - axis_did_move = didmove; + // Set flags for all moving axes, accounting for kinematics + set_axis_moved_for_current_block(); // No acceleration / deceleration time elapsed so far acceleration_time = deceleration_time = 0; @@ -3258,15 +3293,12 @@ void Stepper::_set_position(const abce_long_t &spos) { #endif #if ANY(IS_CORE, MARKFORGED_XY, MARKFORGED_YX) + // Core equations follow the form of the dA and dB equations at https://www.corexy.com/theory.html #if CORE_IS_XY - // corexy positioning - // these equations follow the form of the dA and dB equations on https://www.corexy.com/theory.html count_position.set(spos.a + spos.b, CORESIGN(spos.a - spos.b) OPTARG(HAS_Z_AXIS, spos.c)); #elif CORE_IS_XZ - // corexz planning count_position.set(spos.a + spos.c, spos.b, CORESIGN(spos.a - spos.c)); #elif CORE_IS_YZ - // coreyz planning count_position.set(spos.a, spos.b + spos.c, CORESIGN(spos.b - spos.c)); #elif ENABLED(MARKFORGED_XY) count_position.set(spos.a TERN(MARKFORGED_INVERSE, +, -) spos.b, spos.b, spos.c); @@ -3462,16 +3494,24 @@ void Stepper::report_positions() { #if ENABLED(FT_MOTION) - // Set stepper I/O for fixed time controller. + /** + * Run stepping from the Stepper ISR at regular short intervals. + * + * - Set ftMotion.sts_stepperBusy state to reflect whether there are any commands in the circular buffer. + * - If there are no commands in the buffer, return. + * - Get the next command from the circular buffer ftMotion.stepperCmdBuff[]. + * - If the block is being aborted, return without processing the command. + * - Apply STEP/DIR along with any delays required. A command may be empty, with no STEP/DIR. + */ void Stepper::ftMotion_stepper() { + static AxisBits direction_bits{0}; + // Check if the buffer is empty. ftMotion.sts_stepperBusy = (ftMotion.stepperCmdBuff_produceIdx != ftMotion.stepperCmdBuff_consumeIdx); if (!ftMotion.sts_stepperBusy) return; // "Pop" one command from current motion buffer - // Use one byte to restore one stepper command in the format: - // |X_step|X_direction|Y_step|Y_direction|Z_step|Z_direction|E_step|E_direction| const ft_command_t command = ftMotion.stepperCmdBuff[ftMotion.stepperCmdBuff_consumeIdx]; if (++ftMotion.stepperCmdBuff_consumeIdx == (FTM_STEPPERCMD_BUFF_SIZE)) ftMotion.stepperCmdBuff_consumeIdx = 0; @@ -3480,58 +3520,80 @@ void Stepper::report_positions() { USING_TIMED_PULSE(); - axis_did_move = LOGICAL_AXIS_ARRAY( + // Get FT Motion command flags for axis STEP / DIR + #define _FTM_STEP(AXIS) TEST(command, FT_BIT_STEP_##AXIS) + #define _FTM_DIR(AXIS) TEST(command, FT_BIT_DIR_##AXIS) + + AxisBits axis_step; + axis_step = LOGICAL_AXIS_ARRAY( TEST(command, FT_BIT_STEP_E), TEST(command, FT_BIT_STEP_X), TEST(command, FT_BIT_STEP_Y), TEST(command, FT_BIT_STEP_Z), TEST(command, FT_BIT_STEP_I), TEST(command, FT_BIT_STEP_J), TEST(command, FT_BIT_STEP_K), TEST(command, FT_BIT_STEP_U), TEST(command, FT_BIT_STEP_V), TEST(command, FT_BIT_STEP_W) ); - 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 + direction_bits = LOGICAL_AXIS_ARRAY( + axis_step.e ? TEST(command, FT_BIT_DIR_E) : direction_bits.e, + axis_step.x ? TEST(command, FT_BIT_DIR_X) : direction_bits.x, + axis_step.y ? TEST(command, FT_BIT_DIR_Y) : direction_bits.y, + axis_step.z ? TEST(command, FT_BIT_DIR_Z) : direction_bits.z, + axis_step.i ? TEST(command, FT_BIT_DIR_I) : direction_bits.i, + axis_step.j ? TEST(command, FT_BIT_DIR_J) : direction_bits.j, + axis_step.k ? TEST(command, FT_BIT_DIR_K) : direction_bits.k, + axis_step.u ? TEST(command, FT_BIT_DIR_U) : direction_bits.u, + axis_step.v ? TEST(command, FT_BIT_DIR_V) : direction_bits.v, + axis_step.w ? TEST(command, FT_BIT_DIR_W) : direction_bits.w ); // Apply directions (which will apply to the entire linear move) 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) + E_APPLY_DIR(direction_bits.e, false), + X_APPLY_DIR(direction_bits.x, false), Y_APPLY_DIR(direction_bits.y, false), Z_APPLY_DIR(direction_bits.z, false), + I_APPLY_DIR(direction_bits.i, false), J_APPLY_DIR(direction_bits.j, false), K_APPLY_DIR(direction_bits.k, false), + U_APPLY_DIR(direction_bits.u, false), V_APPLY_DIR(direction_bits.v, false), W_APPLY_DIR(direction_bits.w, false) ); - DIR_WAIT_AFTER(); + /** + * Update direction bits for steppers that were stepped by this command. + * HX, HY, HZ direction bits were set for Core kinematics + * when the block was fetched and are not overwritten here. + */ // Start a step pulse LOGICAL_AXIS_CODE( - 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) + E_APPLY_STEP(axis_step.e, false), + X_APPLY_STEP(axis_step.x, false), Y_APPLY_STEP(axis_step.y, false), Z_APPLY_STEP(axis_step.z, false), + I_APPLY_STEP(axis_step.i, false), J_APPLY_STEP(axis_step.j, false), K_APPLY_STEP(axis_step.k, false), + U_APPLY_STEP(axis_step.u, false), V_APPLY_STEP(axis_step.v, false), W_APPLY_STEP(axis_step.w, false) ); + if (TERN1(FTM_OPTIMIZE_DIR_STATES, last_set_direction != last_direction_bits)) { + // Apply directions (generally applying to the entire linear move) + #define _FTM_APPLY_DIR(AXIS) if (TERN1(FTM_OPTIMIZE_DIR_STATES, last_direction_bits[_AXIS(A)] != last_set_direction[_AXIS(AXIS)])) \ + SET_STEP_DIR(AXIS); + LOGICAL_AXIS_MAP(_FTM_APPLY_DIR); + + TERN_(FTM_OPTIMIZE_DIR_STATES, last_set_direction = last_direction_bits); + + // Any DIR change requires a wait period + DIR_WAIT_AFTER(); + } + + // Start step pulses. Edge stepping will toggle the STEP pin. + #define _FTM_STEP_START(AXIS) AXIS##_APPLY_STEP(_FTM_STEP(AXIS), false); + LOGICAL_AXIS_MAP(_FTM_STEP_START); + + // Apply steps via I2S TERN_(I2S_STEPPER_STREAM, i2s_push_sample()); // Begin waiting for the minimum pulse duration START_TIMED_PULSE(); // Update step counts - LOGICAL_AXIS_CODE( - 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 - ); + #define _FTM_STEP_COUNT(AXIS) if (axis_step[_AXIS(AXIS)]) count_position[_AXIS(AXIS)] += direction_bits[_AXIS(AXIS)] ? 1 : -1; + LOGICAL_AXIS_MAP(_FTM_STEP_COUNT); + // Provide EDGE flags for E stepper(s) #if HAS_EXTRUDERS #if ENABLED(E_DUAL_STEPPER_DRIVERS) constexpr bool e_axis_has_dedge = AXIS_HAS_DEDGE(E0) && AXIS_HAS_DEDGE(E1); @@ -3544,38 +3606,32 @@ void Stepper::report_positions() { // Only wait for axes without edge stepping const bool any_wait = false LOGICAL_AXIS_GANG( - || (!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) + || (!e_axis_has_dedge && axis_step.e), + || (!AXIS_HAS_DEDGE(X) && axis_step.x), || (!AXIS_HAS_DEDGE(Y) && axis_step.y), || (!AXIS_HAS_DEDGE(Z) && axis_step.z), + || (!AXIS_HAS_DEDGE(I) && axis_step.i), || (!AXIS_HAS_DEDGE(J) && axis_step.j), || (!AXIS_HAS_DEDGE(K) && axis_step.k), + || (!AXIS_HAS_DEDGE(U) && axis_step.u), || (!AXIS_HAS_DEDGE(V) && axis_step.v), || (!AXIS_HAS_DEDGE(W) && axis_step.w) ); // Allow pulses to be registered by stepper drivers if (any_wait) AWAIT_HIGH_PULSE(); // Stop pulses. Axes with DEDGE will do nothing, assuming STEP_STATE_* is HIGH - LOGICAL_AXIS_CODE( - E_APPLY_STEP(!STEP_STATE_E, false), - X_APPLY_STEP(!STEP_STATE_X, false), Y_APPLY_STEP(!STEP_STATE_Y, false), Z_APPLY_STEP(!STEP_STATE_Z, false), - I_APPLY_STEP(!STEP_STATE_I, false), J_APPLY_STEP(!STEP_STATE_J, false), K_APPLY_STEP(!STEP_STATE_K, false), - U_APPLY_STEP(!STEP_STATE_U, false), V_APPLY_STEP(!STEP_STATE_V, false), W_APPLY_STEP(!STEP_STATE_W, false) - ); - - // Check endstops on every step - IF_DISABLED(ENDSTOP_INTERRUPTS_FEATURE, endstops.update()); + #define _FTM_STEP_STOP(AXIS) AXIS##_APPLY_STEP(!STEP_STATE_##AXIS, false); + LOGICAL_AXIS_MAP(_FTM_STEP_STOP); // Also handle babystepping here TERN_(BABYSTEPPING, if (babystep.has_steps()) babystepping_isr()); } // Stepper::ftMotion_stepper + // Called from FTMotion::loop (when !blockProcRdy) which is called from Marlin idle() void Stepper::ftMotion_blockQueueUpdate() { if (current_block) { - // If the current block is not done processing, return right away + // If the current block is not done processing, return right away. + // A block is done processing when the command buffer has been + // filled, not necessarily when it's done running. if (!ftMotion.getBlockProcDn()) return; - - axis_did_move.reset(); planner.release_current_block(); } @@ -3583,10 +3639,37 @@ void Stepper::report_positions() { 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); + // Sync position, fan power, laser power? + while (current_block->is_sync()) { + + #if 0 + + // TODO: Implement compatible sync blocks with FT Motion commands, + // perhaps by setting a FT_BIT_SYNC flag that holds the current block + // until it is processed by ftMotion_stepper + + // Set laser power + #if ENABLED(LASER_POWER_SYNC) + if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { + if (current_block->is_sync_pwr()) { + planner.laser_inline.status.isSyncPower = true; + cutter.apply_power(current_block->laser.power); + } + } + #endif + + // Set "fan speeds" for a laser module + #if ENABLED(LASER_SYNCHRONOUS_M106_M107) + if (current_block->is_sync_fan()) planner.sync_fan_speeds(current_block->fan_speed); + #endif + + // Set position + if (current_block->is_sync_pos()) _set_position(current_block->position); + + #endif + + // Done with this block planner.release_current_block(); // Try to get a new block @@ -3594,6 +3677,17 @@ void Stepper::report_positions() { return; // No queued blocks. } + // Some kinematics track axis motion in HX, HY, HZ + #if ANY(CORE_IS_XY, CORE_IS_XZ, MARKFORGED_XY, MARKFORGED_YX) + last_direction_bits.hx = current_block->direction_bits.hx; + #endif + #if ANY(CORE_IS_XY, CORE_IS_YZ, MARKFORGED_XY, MARKFORGED_YX) + last_direction_bits.hy = current_block->direction_bits.hy; + #endif + #if ANY(CORE_IS_XZ, CORE_IS_YZ) + last_direction_bits.hz = current_block->direction_bits.hz; + #endif + ftMotion.startBlockProc(); return; } diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 84f85391d2..43d1012f10 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -684,6 +684,9 @@ class Stepper { // Calculate timing interval and steps-per-ISR for the given step rate static hal_timer_t calc_multistep_timer_interval(uint32_t step_rate); + // Evaluate axis motions and set bits in axis_did_move + static void set_axis_moved_for_current_block(); + #if ENABLED(NONLINEAR_EXTRUSION) static void calc_nonlinear_e(uint32_t step_rate); #endif