Skip to content

Commit

Permalink
Tweak axis move tracking
Browse files Browse the repository at this point in the history
  • Loading branch information
@thinkyhead
thinkyhead committed Aug 13, 2024
1 parent 96a04bc commit 45092a1
Show file tree
Hide file tree
Showing 3 changed files with 49 additions and 66 deletions.
34 changes: 32 additions & 2 deletions Marlin/src/core/types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,16 @@ template <class L, class R> struct IF<true, L, R> { typedef L type; };
#define SECONDARY_AXIS_GANG(V...) GANG_N(SECONDARY_AXES, V)
#define SECONDARY_AXIS_CODE(V...) CODE_N(SECONDARY_AXES, V)
#define SECONDARY_AXIS_LIST(V...) LIST_N(SECONDARY_AXES, V)
#define SECONDARY_AXIS_ARGS(T) SECONDARY_AXIS_LIST(T i, T j, T k, T u, T v, T w)
#if SECONDARY_AXES
#define SECONDARY_AXIS_NAMES SECONDARY_AXIS_LIST(I, J, K, U, V, W)
#define SECONDARY_AXIS_NAMES_LC SECONDARY_AXIS_LIST(i, j, k, u, v, w)
#define SECONDARY_AXIS_ARGS(T) SECONDARY_AXIS_LIST(T i, T j, T k, T u, T v, T w)
#define SECONDARY_AXIS_MAP(F) MAP(F, SECONDARY_AXIS_NAMES)
#define SECONDARY_AXIS_MAP_LC(F) MAP(F, SECONDARY_AXIS_NAMES_LC)
#else
#define SECONDARY_AXIS_MAP(F)
#define SECONDARY_AXIS_MAP_LC(F)
#endif

// Just the XY or XYZ elements
#if HAS_Z_AXIS
Expand Down Expand Up @@ -1048,6 +1057,25 @@ class AxisBits {
};
};

class BitProxy {
public:
BitProxy(el& data, int bit) : data_(data), bit_(bit) {}

BitProxy& operator=(const bool value) {
if (value)
data_ |= (el(1) << bit_);
else
data_ &= ~(el(1) << bit_);
return *this;
}

operator bool() const { return bool(data_ & (el(1) << bit_)); }

private:
el& data_;
uint8_t bit_;
};

AxisBits() { reset(); }

// Constructor, setter, and operator= for bit mask
Expand Down Expand Up @@ -1148,7 +1176,9 @@ class AxisBits {
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); }
FI BitProxy operator[](const int n) { return BitProxy(bits, n); }
FI BitProxy operator[](const AxisEnum n) { return BitProxy(bits, n); }
FI bool operator[](const int n) const { return TEST(bits, n); }
FI bool operator[](const AxisEnum n) const { return TEST(bits, n); }

FI AxisBits& operator|=(const el &p) { bits |= el(p); return *this; }
Expand Down
73 changes: 13 additions & 60 deletions Marlin/src/module/ft_motion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,8 +45,8 @@ int32_t FTMotion::stepperCmdBuff_produceIdx = 0, // Index of next stepper comman

bool FTMotion::sts_stepperBusy = false; // The stepper buffer has items and is in use.

millis_t FTMotion::axis_pos_move_end_ti[NUM_AXIS_HEADS] = {0},
FTMotion::axis_neg_move_end_ti[NUM_AXIS_HEADS] = {0};
millis_t FTMotion::axis_move_end_ti[NUM_AXES] = {0};
AxisBits FTMotion::axis_move_dir;

// Private variables.

Expand Down Expand Up @@ -392,38 +392,7 @@ void FTMotion::reset() {

TERN_(HAS_EXTRUDERS, e_raw_z1 = e_advanced_z1 = 0.0f);

NUM_AXIS_CODE(
axis_pos_move_end_ti[A_AXIS] = 0,
axis_pos_move_end_ti[B_AXIS] = 0,
axis_pos_move_end_ti[C_AXIS] = 0,
axis_pos_move_end_ti[I_AXIS] = 0,
axis_pos_move_end_ti[J_AXIS] = 0,
axis_pos_move_end_ti[K_AXIS] = 0,
axis_pos_move_end_ti[U_AXIS] = 0,
axis_pos_move_end_ti[V_AXIS] = 0,
axis_pos_move_end_ti[W_AXIS] = 0
);
#if ANY(CORE_IS_XY, MARKFORGED_XY, MARKFORGED_YX)
axis_pos_move_end_ti[X_HEAD] = 0;
axis_pos_move_end_ti[Y_HEAD] = 0;
#endif

NUM_AXIS_CODE(
axis_neg_move_end_ti[A_AXIS] = 0,
axis_neg_move_end_ti[B_AXIS] = 0,
axis_neg_move_end_ti[C_AXIS] = 0,
axis_neg_move_end_ti[I_AXIS] = 0,
axis_neg_move_end_ti[J_AXIS] = 0,
axis_neg_move_end_ti[K_AXIS] = 0,
axis_neg_move_end_ti[U_AXIS] = 0,
axis_neg_move_end_ti[V_AXIS] = 0,
axis_neg_move_end_ti[W_AXIS] = 0
);
#if ANY(CORE_IS_XY, MARKFORGED_XY, MARKFORGED_YX)
axis_neg_move_end_ti[X_HEAD] = 0;
axis_neg_move_end_ti[Y_HEAD] = 0;
#endif

ZERO(axis_move_end_ti);
}

void FTMotion::setup_traj_gen(uint32_t intervals) {
Expand Down Expand Up @@ -593,36 +562,20 @@ void FTMotion::loadBlockData(block_t * const current_block) {

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));
// Watch endstops until the move ends
const 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)));

#define __SET_MOVE_END(A,V) do{ if (V) { axis_move_end_ti[_AXIS(A)] = move_end_ti; axis_move_dir[_AXIS(A)] = (V > 0); } }while(0);
#define _SET_MOVE_END(A) __SET_MOVE_END(A, moveDist[_AXIS(A)])
#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;
__SET_MOVE_END(X, moveDist.x + moveDist.y);
__SET_MOVE_END(Y, moveDist.x - moveDist.y);
#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;
_SET_MOVE_END(X);
_SET_MOVE_END(Y);
#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;
// .
// .
// .
TERN_(HAS_Z_AXIS, _SET_MOVE_END(Z));
SECONDARY_AXIS_MAP(_SET_MOVE_END);

// If the endstop is already pressed, endstop interrupts won't invoke
// endstop_triggered and the move will grind. So check here for a
Expand Down
8 changes: 4 additions & 4 deletions Marlin/src/module/ft_motion.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,8 +112,8 @@ 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_HEADS],
axis_neg_move_end_ti[NUM_AXIS_HEADS];
static millis_t axis_move_end_ti[NUM_AXES];
static AxisBits axis_move_dir;

// Public methods
static void init();
Expand All @@ -128,8 +128,8 @@ class FTMotion {

static void setup_traj_gen(uint32_t intervals);

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]); }
static bool axis_moving_pos(const AxisEnum axis) { return axis_move_dir[axis] && !ELAPSED(millis(), axis_move_end_ti[axis]); }
static bool axis_moving_neg(const AxisEnum axis) { return !axis_move_dir[axis] && !ELAPSED(millis(), axis_move_end_ti[axis]); }

private:

Expand Down

0 comments on commit 45092a1

Please sign in to comment.