From e64b7a3ab27dd1f0dc56e227b42295e5c931df62 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sat, 1 Feb 2020 00:57:14 -0600 Subject: [PATCH] Clean up i2c encoder, sanitize serial --- Marlin/src/feature/I2CPositionEncoder.cpp | 46 +++++++++++---------- Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp | 4 +- 2 files changed, 27 insertions(+), 23 deletions(-) diff --git a/Marlin/src/feature/I2CPositionEncoder.cpp b/Marlin/src/feature/I2CPositionEncoder.cpp index c3b182c72a..07f3aa2141 100644 --- a/Marlin/src/feature/I2CPositionEncoder.cpp +++ b/Marlin/src/feature/I2CPositionEncoder.cpp @@ -162,15 +162,17 @@ void I2CPositionEncoder::update() { if (errIdx == 0) { // In order to correct for "error" but avoid correcting for noise and non-skips // it must be > threshold and have a difference average of < 10 and be < 2000 steps - if (ABS(error) > threshold * planner.settings.axis_steps_per_mm[encoderAxis] && - diffSum < 10 * (I2CPE_ERR_ARRAY_SIZE - 1) && ABS(error) < 2000) { // Check for persistent error (skip) + if (ABS(error) > threshold * planner.settings.axis_steps_per_mm[encoderAxis] + && diffSum < 10 * (I2CPE_ERR_ARRAY_SIZE - 1) + && ABS(error) < 2000 + ) { // Check for persistent error (skip) errPrst[errPrstIdx++] = error; // Error must persist for I2CPE_ERR_PRST_ARRAY_SIZE error cycles. This also serves to improve the average accuracy if (errPrstIdx >= I2CPE_ERR_PRST_ARRAY_SIZE) { float sumP = 0; LOOP_L_N(i, I2CPE_ERR_PRST_ARRAY_SIZE) sumP += errPrst[i]; const int32_t errorP = int32_t(sumP * RECIPROCAL(I2CPE_ERR_PRST_ARRAY_SIZE)); SERIAL_ECHO(axis_codes[encoderAxis]); - SERIAL_ECHOLNPAIR(" - err detected: ", errorP * planner.steps_to_mm[encoderAxis], "mm; correcting!"); + SERIAL_ECHOLNPAIR(" : CORRECT ERR ", errorP * planner.steps_to_mm[encoderAxis], "mm"); babystep.add_steps(encoderAxis, -LROUND(errorP)); errPrstIdx = 0; } @@ -189,7 +191,8 @@ void I2CPositionEncoder::update() { if (ABS(error) > I2CPE_ERR_CNT_THRESH * planner.settings.axis_steps_per_mm[encoderAxis]) { const millis_t ms = millis(); if (ELAPSED(ms, nextErrorCountTime)) { - SERIAL_ECHOLNPAIR("Large error on ", axis_codes[encoderAxis], " axis. error: ", (int)error, "; diffSum: ", diffSum); + SERIAL_ECHO(axis_codes[encoderAxis]); + SERIAL_ECHOLNPAIR(" : LARGE ERR ", int(error), "; diffSum=", diffSum); errorCount++; nextErrorCountTime = ms + I2CPE_ERR_CNT_DEBOUNCE_MS; } @@ -243,17 +246,14 @@ bool I2CPositionEncoder::passes_test(const bool report) { } float I2CPositionEncoder::get_axis_error_mm(const bool report) { - float target, actual, error; - - target = planner.get_axis_position_mm(encoderAxis); - actual = mm_from_count(position); - error = actual - target; - - if (ABS(error) > 10000) error = 0; // ? + const float target = planner.get_axis_position_mm(encoderAxis), + actual = mm_from_count(position), + diff = actual - target, + error = ABS(diff) > 10000 ? 0 : diff; // Huge error is a bad reading if (report) { SERIAL_ECHO(axis_codes[encoderAxis]); - SERIAL_ECHOLNPAIR(" axis target: ", target, ", actual: ", actual, ", error : ",error); + SERIAL_ECHOLNPAIR(" axis target=", target, "mm; actual=", actual, "mm; err=", error, "mm"); } return error; @@ -278,21 +278,25 @@ int32_t I2CPositionEncoder::get_axis_error_steps(const bool report) { //convert both 'ticks' into same units / base encoderCountInStepperTicksScaled = LROUND((stepperTicksPerUnit * encoderTicks) / encoderTicksPerUnit); - int32_t target = stepper.position(encoderAxis), - error = (encoderCountInStepperTicksScaled - target); + const int32_t target = stepper.position(encoderAxis); + int32_t error = encoderCountInStepperTicksScaled - target; //suppress discontinuities (might be caused by bad I2C readings...?) const bool suppressOutput = (ABS(error - errorPrev) > 100); - if (report) { - SERIAL_ECHO(axis_codes[encoderAxis]); - SERIAL_ECHOLNPAIR(" axis target: ", target, ", actual: ", encoderCountInStepperTicksScaled, ", error : ", error); - if (suppressOutput) SERIAL_ECHOLNPGM("Discontinuity detected, suppressing error."); - } - errorPrev = error; - return (suppressOutput ? 0 : error); + if (report) { + SERIAL_ECHO(axis_codes[encoderAxis]); + SERIAL_ECHOLNPAIR(" axis target=", target, "; actual=", encoderCountInStepperTicksScaled, "; err=", error); + } + + if (suppressOutput) { + if (report) SERIAL_ECHOLNPGM("!Discontinuity. Suppressing error."); + error = 0; + } + + return error; } int32_t I2CPositionEncoder::get_raw_count() { diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp index 203d7c7150..c8f3b45b0d 100644 --- a/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp +++ b/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp @@ -1629,7 +1629,7 @@ auto debug_pt = [](PGM_P const pre, const xy_pos_t &pos, const float &zadd) { d_from(); serialprintPGM(pre); DEBUG_ECHO_F(normed(pos, zadd), 6); - DEBUG_ECHOLNPAIR_F(" Z error: ", zadd - get_z_correction(pos), 6); + DEBUG_ECHOLNPAIR_F(" Z error = ", zadd - get_z_correction(pos), 6); }; debug_pt(PSTR("1st point: "), probe_pt[0], normal.z * z1); debug_pt(PSTR("2nd point: "), probe_pt[1], normal.z * z2); @@ -1638,7 +1638,7 @@ DEBUG_ECHOLNPAIR_F("0 : ", normed(safe_homing_xy, 0), 6); d_from(); DEBUG_ECHOPGM("safe home with Z="); DEBUG_ECHOLNPAIR_F("mesh value ", normed(safe_homing_xy, get_z_correction(safe_homing_xy)), 6); - DEBUG_ECHOPAIR(" Z error: (", Z_SAFE_HOMING_X_POINT, ",", Z_SAFE_HOMING_Y_POINT); + DEBUG_ECHOPAIR(" Z error = (", Z_SAFE_HOMING_X_POINT, ",", Z_SAFE_HOMING_Y_POINT); DEBUG_ECHOLNPAIR_F(") = ", get_z_correction(safe_homing_xy), 6); #endif } // DEBUGGING(LEVELING)