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 38ee89d commit 52bc3db
Show file tree
Hide file tree
Showing 4 changed files with 96 additions and 86 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
57 changes: 37 additions & 20 deletions Marlin/src/module/endstops.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,9 @@
#include "temperature.h"
#include "../lcd/marlinui.h"

#define DEBUG_OUT ALL(USE_SENSORLESS, DEBUG_LEVELING_FEATURE)
#include "../core/debug_out.h"
#if ENABLED(FT_MOTION)
#include "ft_motion.h"
#endif

#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
#include HAL_PATH(.., endstop_interrupts.h)
Expand All @@ -54,6 +55,9 @@
#include "probe.h"
#endif

#define DEBUG_OUT ALL(USE_SENSORLESS, DEBUG_LEVELING_FEATURE)
#include "../core/debug_out.h"

Endstops endstops;

// private:
Expand Down Expand Up @@ -831,11 +835,16 @@ void Endstops::update() {
#endif
}
#endif

// Signal, after validation, if an endstop limit is pressed or not

#define AXIS_IS_MOVING(A) TERN(FT_MOTION, ftMotion, stepper).axis_is_moving(_AXIS(A))
#define MOTOR_REVERSE(A) !TERN(FT_MOTION, ftMotion, stepper).motor_direction(A)

#if HAS_X_AXIS
if (stepper.axis_is_moving(X_AXIS)) {
if (!stepper.motor_direction(X_AXIS_HEAD)) { // -direction
if (AXIS_IS_MOVING(X_AXIS)) {
const AxisEnum x_head = TERN0(FT_MOTION, ftMotion.cfg.active) ? X_AXIS : X_AXIS_HEAD;
if (MOTOR_REVERSE(x_head)) { // -direction
#if HAS_X_MIN_STATE
PROCESS_ENDSTOP_X(MIN);
#if CORE_DIAG(XY, Y, MIN)
Expand Down Expand Up @@ -867,8 +876,9 @@ 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 (AXIS_IS_MOVING(Y_AXIS)) {
const AxisEnum y_head = TERN0(FT_MOTION, ftMotion.cfg.active) ? Y_AXIS : Y_AXIS_HEAD;
if (MOTOR_REVERSE(y_head)) { // -direction
#if HAS_Y_MIN_STATE
PROCESS_ENDSTOP_Y(MIN);
#if CORE_DIAG(XY, X, MIN)
Expand Down Expand Up @@ -900,8 +910,9 @@ 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 (AXIS_IS_MOVING(Z_AXIS)) {
const AxisEnum z_head = TERN0(FT_MOTION, ftMotion.cfg.active) ? Z_AXIS : Z_AXIS_HEAD;
if (MOTOR_REVERSE(z_head)) { // 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
Expand Down Expand Up @@ -945,8 +956,9 @@ void Endstops::update() {
#endif // HAS_Z_AXIS

#if HAS_I_AXIS
if (stepper.axis_is_moving(I_AXIS)) {
if (!stepper.motor_direction(I_AXIS_HEAD)) { // -direction
if (AXIS_IS_MOVING(I_AXIS)) {
const AxisEnum i_head = TERN0(FT_MOTION, ftMotion.cfg.active) ? I_AXIS : I_AXIS_HEAD;
if (MOTOR_REVERSE(i_head)) { // -direction
#if HAS_I_MIN_STATE
PROCESS_ENDSTOP(I, MIN);
#endif
Expand All @@ -960,8 +972,9 @@ void Endstops::update() {
#endif // HAS_I_AXIS

#if HAS_J_AXIS
if (stepper.axis_is_moving(J_AXIS)) {
if (!stepper.motor_direction(J_AXIS_HEAD)) { // -direction
if (AXIS_IS_MOVING(J_AXIS)) {
const AxisEnum j_head = TERN0(FT_MOTION, ftMotion.cfg.active) ? J_AXIS : J_AXIS_HEAD;
if (MOTOR_REVERSE(j_head)) { // -direction
#if HAS_J_MIN_STATE
PROCESS_ENDSTOP(J, MIN);
#endif
Expand All @@ -975,8 +988,9 @@ void Endstops::update() {
#endif // HAS_J_AXIS

#if HAS_K_AXIS
if (stepper.axis_is_moving(K_AXIS)) {
if (!stepper.motor_direction(K_AXIS_HEAD)) { // -direction
if (AXIS_IS_MOVING(K_AXIS)) {
const AxisEnum k_head = TERN0(FT_MOTION, ftMotion.cfg.active) ? K_AXIS : K_AXIS_HEAD;
if (MOTOR_REVERSE(k_head)) { // -direction
#if HAS_K_MIN_STATE
PROCESS_ENDSTOP(K, MIN);
#endif
Expand All @@ -990,8 +1004,9 @@ void Endstops::update() {
#endif // HAS_K_AXIS

#if HAS_U_AXIS
if (stepper.axis_is_moving(U_AXIS)) {
if (!stepper.motor_direction(U_AXIS_HEAD)) { // -direction
if (AXIS_IS_MOVING(U_AXIS)) {
const AxisEnum u_head = TERN0(FT_MOTION, ftMotion.cfg.active) ? U_AXIS : U_AXIS_HEAD;
if (MOTOR_REVERSE(u_head)) { // -direction
#if HAS_U_MIN_STATE
PROCESS_ENDSTOP(U, MIN);
#endif
Expand All @@ -1005,8 +1020,9 @@ void Endstops::update() {
#endif // HAS_U_AXIS

#if HAS_V_AXIS
if (stepper.axis_is_moving(V_AXIS)) {
if (!stepper.motor_direction(V_AXIS_HEAD)) { // -direction
if (AXIS_IS_MOVING(V_AXIS)) {
const AxisEnum v_head = TERN0(FT_MOTION, ftMotion.cfg.active) ? V_AXIS : V_AXIS_HEAD;
if (MOTOR_REVERSE(v_head)) { // -direction
#if HAS_V_MIN_STATE
PROCESS_ENDSTOP(V, MIN);
#endif
Expand All @@ -1020,8 +1036,9 @@ void Endstops::update() {
#endif // HAS_V_AXIS

#if HAS_W_AXIS
if (stepper.axis_is_moving(W_AXIS)) {
if (!stepper.motor_direction(W_AXIS_HEAD)) { // -direction
if (AXIS_IS_MOVING(W_AXIS)) {
const AxisEnum w_head = TERN0(FT_MOTION, ftMotion.cfg.active) ? W_AXIS : W_AXIS_HEAD;
if (MOTOR_REVERSE(w_head)) { // -direction
#if HAS_W_MIN_STATE
PROCESS_ENDSTOP(W, MIN);
#endif
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 @@ -595,36 +564,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
18 changes: 14 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,18 @@ 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]); }
FORCE_INLINE static bool axis_is_moving(const AxisEnum axis) {
return cfg.active ? PENDING(millis(), axis_move_end_ti[axis]) : stepper.axis_is_moving(axis);
}
FORCE_INLINE static bool motor_direction(const AxisEnum axis) {
return cfg.active ? axis_move_dir[axis] : stepper.last_direction_bits[axis];
}
FORCE_INLINE static bool axis_moving_pos(const AxisEnum axis) {
return axis_move_dir[axis] && PENDING(millis(), axis_move_end_ti[axis]);
}
FORCE_INLINE static bool axis_moving_neg(const AxisEnum axis) {
return !axis_move_dir[axis] && PENDING(millis(), axis_move_end_ti[axis]);
}

private:

Expand Down

0 comments on commit 52bc3db

Please sign in to comment.