mirror of
https://github.com/MarlinFirmware/Marlin.git
synced 2025-03-13 01:40:09 +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
GitHub
parent
a3960dfa53
commit
1da947f548
11 changed files with 414 additions and 232 deletions
|
|
@ -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); }
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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(
|
||||
|
|
|
|||
|
|
@ -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()); }
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue