Skip to content

Commit

Permalink
πŸ§‘β€πŸ’» Change Marlin DIR bits: 1=Forward, 0=Reverse (MarlinFirmware#25791)
Browse files Browse the repository at this point in the history
  • Loading branch information
thinkyhead authored and Andy-Big committed Jul 15, 2023
1 parent d3c2401 commit 3fffc69
Show file tree
Hide file tree
Showing 11 changed files with 260 additions and 320 deletions.
10 changes: 5 additions & 5 deletions Marlin/src/feature/backlash.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -99,15 +99,15 @@ void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const

LOOP_NUM_AXES(axis) {
if (distance_mm[axis]) {
const bool reverse = dm[axis];
const bool forward = dm[axis];

// When an axis changes direction, add axis backlash to the residual error
if (changed_dir[axis])
residual_error[axis] += (reverse ? -f_corr : f_corr) * distance_mm[axis] * planner.settings.axis_steps_per_mm[axis];
residual_error[axis] += (forward ? f_corr : -f_corr) * distance_mm[axis] * planner.settings.axis_steps_per_mm[axis];

// Decide how much of the residual error to correct in this segment
int32_t error_correction = residual_error[axis];
if (reverse != (error_correction < 0))
if (forward == (error_correction < 0))
error_correction = 0; // Don't take up any backlash in this segment, as it would subtract steps

#ifdef BACKLASH_SMOOTHING_MM
Expand Down Expand Up @@ -147,14 +147,14 @@ void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const
int32_t Backlash::get_applied_steps(const AxisEnum axis) {
if (axis >= NUM_AXES) return 0;

const bool reverse = last_direction_bits[axis];
const bool forward = last_direction_bits[axis];

const int32_t residual_error_axis = residual_error[axis];

// At startup it is assumed the last move was forwards. So the applied
// steps will always be a non-positive number.

if (!reverse) return -residual_error_axis;
if (forward) return -residual_error_axis;

const float f_corr = float(correction) / all_on;
const int32_t full_error_axis = -f_corr * distance_mm[axis] * planner.settings.axis_steps_per_mm[axis];
Expand Down
2 changes: 1 addition & 1 deletion Marlin/src/feature/runout.h
Original file line number Diff line number Diff line change
Expand Up @@ -411,7 +411,7 @@ class FilamentSensorBase {
// Only trigger on extrusion with XYZ movement to allow filament change and retract/recover.
const uint8_t e = b->extruder;
const int32_t steps = b->steps.e;
const float mm = (b->direction_bits.e ? -steps : steps) * planner.mm_per_step[E_AXIS_N(e)];
const float mm = (b->direction_bits.e ? steps : -steps) * planner.mm_per_step[E_AXIS_N(e)];
if (e < NUM_RUNOUT_SENSORS) mm_countdown.runout[e] -= mm;
#if ENABLED(FILAMENT_SWITCH_AND_MOTION)
if (e < NUM_MOTION_SENSORS) mm_countdown.motion[e] -= mm;
Expand Down
13 changes: 8 additions & 5 deletions Marlin/src/gcode/feature/ft_motion/M493.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -102,8 +102,10 @@ void GcodeSuite::M493() {
}

switch (val) {
case ftMotionMode_ENABLED: fxdTiCtrl.reset(); break;
#if HAS_X_AXIS
//case ftMotionMode_ULENDO_FBS:
//case ftMotionMode_DISCTF:
// break;
case ftMotionMode_ZV:
case ftMotionMode_ZVD:
case ftMotionMode_EI:
Expand All @@ -114,9 +116,10 @@ void GcodeSuite::M493() {
fxdTiCtrl.updateShapingA();
fxdTiCtrl.reset();
break;
//case ftMotionMode_ULENDO_FBS:
//case ftMotionMode_DISCTF:
#endif
case ftMotionMode_ENABLED:
fxdTiCtrl.reset();
break;
default: break;
}
}
Expand Down Expand Up @@ -195,7 +198,7 @@ void GcodeSuite::M493() {
fxdTiCtrl.reset();
if (fxdTiCtrl.cfg_dynFreqMode) { SERIAL_ECHOPGM("Compensator base dynamic frequency (X/A axis) set to:"); }
else { SERIAL_ECHOPGM("Compensator static frequency (X/A axis) set to: "); }
SERIAL_ECHO_F( fxdTiCtrl.cfg_baseFreq[0], 2 );
SERIAL_ECHO_F(fxdTiCtrl.cfg_baseFreq[0], 2);
SERIAL_ECHOLNPGM(".");
}
else { // Frequency out of range.
Expand Down Expand Up @@ -243,7 +246,7 @@ void GcodeSuite::M493() {
fxdTiCtrl.reset();
if (fxdTiCtrl.cfg_dynFreqMode) { SERIAL_ECHOPGM("Compensator base dynamic frequency (Y/B axis) set to:"); }
else { SERIAL_ECHOPGM("Compensator static frequency (Y/B axis) set to: "); }
SERIAL_ECHO_F( fxdTiCtrl.cfg_baseFreq[1], 2 );
SERIAL_ECHO_F(fxdTiCtrl.cfg_baseFreq[1], 2);
SERIAL_ECHOLNPGM(".");
}
else { // Frequency out of range.
Expand Down
11 changes: 7 additions & 4 deletions Marlin/src/gcode/motion/G6.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,10 +38,13 @@ void GcodeSuite::G6() {
planner.last_page_step_rate = parser.value_ulong();

if (!DirectStepping::Config::DIRECTIONAL) {
if (parser.seen('X')) planner.last_page_dir.x = !!parser.value_byte();
if (parser.seen('Y')) planner.last_page_dir.y = !!parser.value_byte();
if (parser.seen('Z')) planner.last_page_dir.z = !!parser.value_byte();
if (parser.seen('E')) planner.last_page_dir.e = !!parser.value_byte();
#define PAGE_DIR_SET(N,A) do{ if (parser.seen(N)) planner.last_page_dir.A = !!parser.value_byte(); } while(0)
LOGICAL_AXIS_CODE(
PAGE_DIR_SET('E',E),
PAGE_DIR_SET('X',X), PAGE_DIR_SET('Y',Y), PAGE_DIR_SET('Z',Z),
PAGE_DIR_SET(AXIS4_NAME,I), PAGE_DIR_SET(AXIS5_NAME,J), PAGE_DIR_SET(AXIS6_NAME,K),
PAGE_DIR_SET(AXIS5_NAME,U), PAGE_DIR_SET(AXIS6_NAME,V), PAGE_DIR_SET(AXIS7_NAME,W)
);
}

// No index means we just set the state
Expand Down
8 changes: 6 additions & 2 deletions Marlin/src/inc/SanityCheck.h
Original file line number Diff line number Diff line change
Expand Up @@ -4036,8 +4036,12 @@ static_assert(_PLUS_TEST(3), "DEFAULT_MAX_ACCELERATION values must be positive."
/**
* Fixed-Time Motion limitations
*/
#if ENABLED(FT_MOTION) && (NUM_AXES > 3 || E_STEPPERS > 1 || NUM_Z_STEPPERS > 1 || ANY(DUAL_X_CARRIAGE, HAS_SYNCED_X_STEPPERS, HAS_SYNCED_Y_STEPPERS, HAS_MULTI_EXTRUDER, MIXING_EXTRUDER))
#error "FT_MOTION is currently limited to machines with 3 linear axes and a single extruder."
#if ENABLED(FT_MOTION)
#if NUM_AXES > 3
#error "FT_MOTION is currently limited to machines with 3 linear axes."
#elif ENABLED(MIXING_EXTRUDER)
#error "FT_MOTION is incompatible with MIXING_EXTRUDER."
#endif
#endif

// Multi-Stepping Limit
Expand Down
18 changes: 9 additions & 9 deletions Marlin/src/module/endstops.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -874,7 +874,7 @@ void Endstops::update() {

#if HAS_X_AXIS
if (stepper.axis_is_moving(X_AXIS)) {
if (stepper.motor_direction(X_AXIS_HEAD)) { // -direction
if (!stepper.motor_direction(X_AXIS_HEAD)) { // -direction
#if USE_X_MIN || (X_SPI_SENSORLESS && X_HOME_TO_MIN)
PROCESS_ENDSTOP_X(MIN);
#if CORE_DIAG(XY, Y, MIN)
Expand Down Expand Up @@ -907,7 +907,7 @@ void Endstops::update() {

#if HAS_Y_AXIS
if (stepper.axis_is_moving(Y_AXIS)) {
if (stepper.motor_direction(Y_AXIS_HEAD)) { // -direction
if (!stepper.motor_direction(Y_AXIS_HEAD)) { // -direction
#if USE_Y_MIN || (Y_SPI_SENSORLESS && Y_HOME_TO_MIN)
PROCESS_ENDSTOP_Y(MIN);
#if CORE_DIAG(XY, X, MIN)
Expand Down Expand Up @@ -940,7 +940,7 @@ void Endstops::update() {

#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 (!stepper.motor_direction(Z_AXIS_HEAD)) { // Z -direction. Gantry down, bed up.

#if USE_Z_MIN || (Z_SPI_SENSORLESS && Z_HOME_TO_MIN)
if ( TERN1(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN, z_probe_enabled)
Expand Down Expand Up @@ -985,7 +985,7 @@ void Endstops::update() {

#if HAS_I_AXIS
if (stepper.axis_is_moving(I_AXIS)) {
if (stepper.motor_direction(I_AXIS_HEAD)) { // -direction
if (!stepper.motor_direction(I_AXIS_HEAD)) { // -direction
#if USE_I_MIN || (I_SPI_SENSORLESS && I_HOME_TO_MIN)
PROCESS_ENDSTOP(I, MIN);
#endif
Expand All @@ -1000,7 +1000,7 @@ void Endstops::update() {

#if HAS_J_AXIS
if (stepper.axis_is_moving(J_AXIS)) {
if (stepper.motor_direction(J_AXIS_HEAD)) { // -direction
if (!stepper.motor_direction(J_AXIS_HEAD)) { // -direction
#if USE_J_MIN || (J_SPI_SENSORLESS && J_HOME_TO_MIN)
PROCESS_ENDSTOP(J, MIN);
#endif
Expand All @@ -1015,7 +1015,7 @@ void Endstops::update() {

#if HAS_K_AXIS
if (stepper.axis_is_moving(K_AXIS)) {
if (stepper.motor_direction(K_AXIS_HEAD)) { // -direction
if (!stepper.motor_direction(K_AXIS_HEAD)) { // -direction
#if USE_K_MIN || (K_SPI_SENSORLESS && K_HOME_TO_MIN)
PROCESS_ENDSTOP(K, MIN);
#endif
Expand All @@ -1030,7 +1030,7 @@ void Endstops::update() {

#if HAS_U_AXIS
if (stepper.axis_is_moving(U_AXIS)) {
if (stepper.motor_direction(U_AXIS_HEAD)) { // -direction
if (!stepper.motor_direction(U_AXIS_HEAD)) { // -direction
#if USE_U_MIN || (U_SPI_SENSORLESS && U_HOME_TO_MIN)
PROCESS_ENDSTOP(U, MIN);
#endif
Expand All @@ -1045,7 +1045,7 @@ void Endstops::update() {

#if HAS_V_AXIS
if (stepper.axis_is_moving(V_AXIS)) {
if (stepper.motor_direction(V_AXIS_HEAD)) { // -direction
if (!stepper.motor_direction(V_AXIS_HEAD)) { // -direction
#if USE_V_MIN || (V_SPI_SENSORLESS && V_HOME_TO_MIN)
PROCESS_ENDSTOP(V, MIN);
#endif
Expand All @@ -1060,7 +1060,7 @@ void Endstops::update() {

#if HAS_W_AXIS
if (stepper.axis_is_moving(W_AXIS)) {
if (stepper.motor_direction(W_AXIS_HEAD)) { // -direction
if (!stepper.motor_direction(W_AXIS_HEAD)) { // -direction
#if USE_W_MIN || (W_SPI_SENSORLESS && W_HOME_TO_MIN)
PROCESS_ENDSTOP(W, MIN);
#endif
Expand Down
57 changes: 30 additions & 27 deletions Marlin/src/module/ft_motion.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -422,33 +422,36 @@ void FxdTiCtrl::reset() {
stepperCmdBuff_produceIdx = stepperCmdBuff_consumeIdx = 0;

for (uint32_t i = 0U; i < (FTM_BATCH_SIZE); i++) { // Reset trajectory history
TERN_(HAS_X_AXIS, xd[i] = 0.0f);
TERN_(HAS_Y_AXIS, yd[i] = 0.0f);
TERN_(HAS_Z_AXIS, zd[i] = 0.0f);
TERN_(HAS_X_AXIS, xd[i] = 0.0f);
TERN_(HAS_Y_AXIS, yd[i] = 0.0f);
TERN_(HAS_Z_AXIS, zd[i] = 0.0f);
TERN_(HAS_EXTRUDERS, ed[i] = 0.0f);
}

blockProcRdy = blockProcRdy_z1 = blockProcDn = false;
batchRdy = batchRdyForInterp = false;
runoutEna = false;

TERN_(HAS_X_AXIS, x_endPosn_prevBlock = 0.0f);
TERN_(HAS_Y_AXIS, y_endPosn_prevBlock = 0.0f);
TERN_(HAS_Z_AXIS, z_endPosn_prevBlock = 0.0f);
TERN_(HAS_X_AXIS, x_endPosn_prevBlock = 0.0f);
TERN_(HAS_Y_AXIS, y_endPosn_prevBlock = 0.0f);
TERN_(HAS_Z_AXIS, z_endPosn_prevBlock = 0.0f);
TERN_(HAS_EXTRUDERS, e_endPosn_prevBlock = 0.0f);

makeVector_idx = makeVector_idx_z1 = 0;
makeVector_batchIdx = FTM_BATCH_SIZE;

TERN_(HAS_X_AXIS, x_steps = 0);
TERN_(HAS_Y_AXIS, y_steps = 0);
TERN_(HAS_Z_AXIS, z_steps = 0);
TERN_(HAS_X_AXIS, x_steps = 0);
TERN_(HAS_Y_AXIS, y_steps = 0);
TERN_(HAS_Z_AXIS, z_steps = 0);
TERN_(HAS_EXTRUDERS, e_steps = 0);

interpIdx = interpIdx_z1 = 0;
TERN_(HAS_X_AXIS, x_dirState = stepDirState_NOT_SET);
TERN_(HAS_Y_AXIS, y_dirState = stepDirState_NOT_SET);
TERN_(HAS_Z_AXIS, z_dirState = stepDirState_NOT_SET);

TERN_(HAS_X_AXIS, x_dirState = stepDirState_NOT_SET);
TERN_(HAS_Y_AXIS, y_dirState = stepDirState_NOT_SET);
TERN_(HAS_Z_AXIS, z_dirState = stepDirState_NOT_SET);
TERN_(HAS_EXTRUDERS, e_dirState = stepDirState_NOT_SET);

nextStepTicks = FTM_MIN_TICKS;

#if HAS_X_AXIS
Expand Down Expand Up @@ -486,28 +489,28 @@ void FxdTiCtrl::loadBlockData(block_t * const current_block) {
#if HAS_X_AXIS
x_startPosn = x_endPosn_prevBlock;
float x_moveDist = current_block->steps.a / planner.settings.axis_steps_per_mm[X_AXIS];
if (direction.x) x_moveDist *= -1.0f;
if (!direction.x) x_moveDist *= -1.0f;
x_Ratio = x_moveDist * oneOverLength;
#endif

#if HAS_Y_AXIS
y_startPosn = y_endPosn_prevBlock;
float y_moveDist = current_block->steps.b / planner.settings.axis_steps_per_mm[Y_AXIS];
if (direction.y) y_moveDist *= -1.0f;
if (!direction.y) y_moveDist *= -1.0f;
y_Ratio = y_moveDist * oneOverLength;
#endif

#if HAS_Z_AXIS
z_startPosn = z_endPosn_prevBlock;
float z_moveDist = current_block->steps.c / planner.settings.axis_steps_per_mm[Z_AXIS];
if (direction.z) z_moveDist *= -1.0f;
if (!direction.z) z_moveDist *= -1.0f;
z_Ratio = z_moveDist * oneOverLength;
#endif

#if HAS_EXTRUDERS
e_startPosn = e_endPosn_prevBlock;
float extrusion = current_block->steps.e / planner.settings.axis_steps_per_mm[E_AXIS_N(current_block->extruder)];
if (direction.e) extrusion *= -1.0f;
if (!direction.e) extrusion *= -1.0f;
e_Ratio = extrusion * oneOverLength;
#endif

Expand Down Expand Up @@ -568,31 +571,31 @@ void FxdTiCtrl::loadBlockData(block_t * const current_block) {
// One less than (Accel + Coasting + Decel) datapoints
max_intervals = N1 + N2 + N3 - 1U;

TERN_(HAS_X_AXIS, x_endPosn_prevBlock += x_moveDist);
TERN_(HAS_Y_AXIS, y_endPosn_prevBlock += y_moveDist);
TERN_(HAS_Z_AXIS, z_endPosn_prevBlock += z_moveDist);
TERN_(HAS_X_AXIS, x_endPosn_prevBlock += x_moveDist);
TERN_(HAS_Y_AXIS, y_endPosn_prevBlock += y_moveDist);
TERN_(HAS_Z_AXIS, z_endPosn_prevBlock += z_moveDist);
TERN_(HAS_EXTRUDERS, e_endPosn_prevBlock += extrusion);
}

// Generate data points of the trajectory.
void FxdTiCtrl::makeVector() {
float accel_k = 0.0f; // (mm/s^2) Acceleration K factor
float tau = (makeVector_idx + 1) * (FTM_TS); // (s) Time since start of block
float dist = 0.0f; // (mm) Distance traveled
float accel_k = 0.0f; // (mm/s^2) Acceleration K factor
float tau = (makeVector_idx + 1) * (FTM_TS); // (s) Time since start of block
float dist = 0.0f; // (mm) Distance traveled

if (makeVector_idx < N1) {
// Acceleration phase
dist = (f_s * tau) + (0.5f * accel_P * sq(tau)); // (mm) Distance traveled for acceleration phase
accel_k = accel_P; // (mm/s^2) Acceleration K factor from Accel phase
dist = (f_s * tau) + (0.5f * accel_P * sq(tau)); // (mm) Distance traveled for acceleration phase
accel_k = accel_P; // (mm/s^2) Acceleration K factor from Accel phase
}
else if (makeVector_idx >= N1 && makeVector_idx < (N1 + N2)) {
// Coasting phase
dist = s_1e + F_P * (tau - N1 * (FTM_TS)); // (mm) Distance traveled for coasting phase
dist = s_1e + F_P * (tau - N1 * (FTM_TS)); // (mm) Distance traveled for coasting phase
//accel_k = 0.0f;
}
else {
// Deceleration phase
const float tau_ = tau - (N1 + N2) * (FTM_TS); // (s) Time since start of decel phase
const float tau_ = tau - (N1 + N2) * (FTM_TS); // (s) Time since start of decel phase
dist = s_2e + F_P * tau_ + 0.5f * decel_P * sq(tau_); // (mm) Distance traveled for deceleration phase
accel_k = decel_P; // (mm/s^2) Acceleration K factor from Decel phase
}
Expand All @@ -614,7 +617,7 @@ void FxdTiCtrl::makeVector() {
}
else {
ed[makeVector_batchIdx] = new_raw_z1;
// Alternatively: coordArray_e[makeVector_batchIdx] = e_startDist + extrusion / (N1 + N2 + N3);
// Alternatively: ed[makeVector_batchIdx] = e_startPosn + (e_Ratio * dist) / (N1 + N2 + N3);
}
#endif

Expand Down
Loading

0 comments on commit 3fffc69

Please sign in to comment.