Backlash Compensation for COREnn (#21612)

Co-authored-by: Scott Lahteine <github@thinkyhead.com>
This commit is contained in:
Nikolay March 2021-04-24 12:38:00 +03:00 committed by GitHub
parent d3a2c6a0b4
commit 6f5800bd74
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 52 additions and 13 deletions

View File

@ -972,6 +972,9 @@
#define BACKLASH_DISTANCE_MM { 0, 0, 0 } // (mm)
#define BACKLASH_CORRECTION 0.0 // 0.0 = no correction; 1.0 = full correction
// Add steps for motor direction changes on CORE kinematics
//#define CORE_BACKLASH
// Set BACKLASH_SMOOTHING_MM to spread backlash correction over multiple segments
// to reduce print artifacts. (Enabling this is costly in memory and computation!)
//#define BACKLASH_SMOOTHING_MM 3 // (mm)

View File

@ -63,10 +63,24 @@ Backlash backlash;
void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const int32_t &dc, const uint8_t dm, block_t * const block) {
static uint8_t last_direction_bits;
uint8_t changed_dir = last_direction_bits ^ dm;
// Ignore direction change if no steps are taken in that direction
if (da == 0) CBI(changed_dir, X_AXIS);
if (db == 0) CBI(changed_dir, Y_AXIS);
if (dc == 0) CBI(changed_dir, Z_AXIS);
// Ignore direction change unless steps are taken in that direction
#if DISABLED(CORE_BACKLASH) || ENABLED(MARKFORGED_XY)
if (!da) CBI(changed_dir, X_AXIS);
if (!db) CBI(changed_dir, Y_AXIS);
if (!dc) CBI(changed_dir, Z_AXIS);
#elif CORE_IS_XY
if (!(da + db)) CBI(changed_dir, X_AXIS);
if (!(da - db)) CBI(changed_dir, Y_AXIS);
if (!dc) CBI(changed_dir, Z_AXIS);
#elif CORE_IS_XZ
if (!(da + dc)) CBI(changed_dir, X_AXIS);
if (!(da - dc)) CBI(changed_dir, Z_AXIS);
if (!db) CBI(changed_dir, Y_AXIS);
#elif CORE_IS_YZ
if (!(db + dc)) CBI(changed_dir, Y_AXIS);
if (!(db - dc)) CBI(changed_dir, Z_AXIS);
if (!da) CBI(changed_dir, X_AXIS);
#endif
last_direction_bits ^= changed_dir;
if (correction == 0) return;
@ -105,18 +119,35 @@ void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const
// Take up a portion of the residual_error in this segment, but only when
// the current segment travels in the same direction as the correction
if (reversing == (error_correction < 0)) {
if (segment_proportion == 0)
segment_proportion = _MIN(1.0f, block->millimeters / smoothing_mm);
if (segment_proportion == 0) segment_proportion = _MIN(1.0f, block->millimeters / smoothing_mm);
error_correction = CEIL(segment_proportion * error_correction);
}
else
error_correction = 0; // Don't take up any backlash in this segment, as it would subtract steps
}
#endif
// Making a correction reduces the residual error and adds block steps
// This correction reduces the residual error and adds block steps
if (error_correction) {
block->steps[axis] += ABS(error_correction);
residual_error[axis] -= error_correction;
#if ENABLED(CORE_BACKLASH)
switch (axis) {
case CORE_AXIS_1:
//block->steps[CORE_AXIS_2] += influence_distance_mm[axis] * planner.settings.axis_steps_per_mm[CORE_AXIS_2];
//SERIAL_ECHOLNPAIR("CORE_AXIS_1 dir change. distance=", distance_mm[axis], " r.err=", residual_error[axis],
// " da=", da, " db=", db, " block->steps[axis]=", block->steps[axis], " err_corr=", error_correction);
break;
case CORE_AXIS_2:
//block->steps[CORE_AXIS_1] += influence_distance_mm[axis] * planner.settings.axis_steps_per_mm[CORE_AXIS_1];;
//SERIAL_ECHOLNPAIR("CORE_AXIS_2 dir change. distance=", distance_mm[axis], " r.err=", residual_error[axis],
// " da=", da, " db=", db, " block->steps[axis]=", block->steps[axis], " err_corr=", error_correction);
break;
case NORMAL_AXIS: break;
}
residual_error[axis] = 0; // No residual_error needed for next CORE block, I think...
#else
residual_error[axis] -= error_correction;
#endif
}
}
}

View File

@ -38,10 +38,15 @@ void menu_backlash() {
EDIT_ITEM_FAST(percent, MSG_BACKLASH_CORRECTION, &backlash.correction, all_off, all_on);
#if DISABLED(CORE_BACKLASH) || ENABLED(MARKFORGED_XY)
#define _CAN_CALI AXIS_CAN_CALIBRATE
#else
#define _CAN_CALI(A) true
#endif
#define EDIT_BACKLASH_DISTANCE(N) EDIT_ITEM_FAST(float43, MSG_BACKLASH_##N, &backlash.distance_mm[_AXIS(N)], 0.0f, 9.9f);
if (AXIS_CAN_CALIBRATE(A)) EDIT_BACKLASH_DISTANCE(A);
if (AXIS_CAN_CALIBRATE(B)) EDIT_BACKLASH_DISTANCE(B);
if (AXIS_CAN_CALIBRATE(C)) EDIT_BACKLASH_DISTANCE(C);
if (_CAN_CALI(A)) EDIT_BACKLASH_DISTANCE(A);
if (_CAN_CALI(B)) EDIT_BACKLASH_DISTANCE(B);
if (_CAN_CALI(C)) EDIT_BACKLASH_DISTANCE(C);
#ifdef BACKLASH_SMOOTHING_MM
EDIT_ITEM_FAST(float43, MSG_BACKLASH_SMOOTHING, &backlash.smoothing_mm, 0.0f, 9.9f);

View File

@ -93,8 +93,8 @@ exec_test $1 $2 "Teensy 3.5/3.6 COREXY" "$3"
#
restore_configs
opt_set MOTHERBOARD BOARD_TEENSY35_36
opt_enable COREXZ
exec_test $1 $2 "Teensy 3.5/3.6 COREXZ" "$3"
opt_enable COREXZ BACKLASH_COMPENSATION BACKLASH_GCODE CORE_BACKLASH
exec_test $1 $2 "Teensy 3.5/3.6 COREXZ | BACKLASH" "$3"
#
# Enable Dual Z with Dual Z endstops