0
0
Fork 0
mirror of https://github.com/MarlinFirmware/Marlin.git synced 2025-03-15 02:36:19 +00:00

️ FT_MOTION : Core and other refinements (#26720)

Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>
Co-authored-by: Ulendo Alex <alex@ulendo.io>
This commit is contained in:
narno2202 2024-05-09 23:57:23 +02:00 committed by GitHub
parent a3960dfa53
commit 1da947f548
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
11 changed files with 414 additions and 232 deletions

View file

@ -46,6 +46,7 @@ template <class L, class R> struct IF<true, L, R> { 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 <class L, class R> struct IF<true, L, R> { 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 <class L, class R> struct IF<true, L, R> { 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 <class L, class R> struct IF<true, L, R> { 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); }

View file

@ -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

View file

@ -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

View file

@ -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

View file

@ -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;

View file

@ -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;

View file

@ -47,7 +47,9 @@ enum dynFreqMode_t : uint8_t {
typedef struct XYZEarray<float, FTM_WINDOW_SIZE> xyze_trajectory_t;
typedef struct XYZEarray<float, FTM_BATCH_SIZE> 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,

View file

@ -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(

View file

@ -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()); }

View file

@ -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;
}

View file

@ -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