Clean up i2c encoder, sanitize serial

This commit is contained in:
Scott Lahteine 2020-02-01 00:57:14 -06:00
parent 1af05797d7
commit e64b7a3ab2
2 changed files with 27 additions and 23 deletions

View File

@ -162,15 +162,17 @@ void I2CPositionEncoder::update() {
if (errIdx == 0) { if (errIdx == 0) {
// In order to correct for "error" but avoid correcting for noise and non-skips // 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 // 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] && 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) && 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 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) { if (errPrstIdx >= I2CPE_ERR_PRST_ARRAY_SIZE) {
float sumP = 0; float sumP = 0;
LOOP_L_N(i, I2CPE_ERR_PRST_ARRAY_SIZE) sumP += errPrst[i]; 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)); const int32_t errorP = int32_t(sumP * RECIPROCAL(I2CPE_ERR_PRST_ARRAY_SIZE));
SERIAL_ECHO(axis_codes[encoderAxis]); 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)); babystep.add_steps(encoderAxis, -LROUND(errorP));
errPrstIdx = 0; errPrstIdx = 0;
} }
@ -189,7 +191,8 @@ void I2CPositionEncoder::update() {
if (ABS(error) > I2CPE_ERR_CNT_THRESH * planner.settings.axis_steps_per_mm[encoderAxis]) { if (ABS(error) > I2CPE_ERR_CNT_THRESH * planner.settings.axis_steps_per_mm[encoderAxis]) {
const millis_t ms = millis(); const millis_t ms = millis();
if (ELAPSED(ms, nextErrorCountTime)) { 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++; errorCount++;
nextErrorCountTime = ms + I2CPE_ERR_CNT_DEBOUNCE_MS; 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 I2CPositionEncoder::get_axis_error_mm(const bool report) {
float target, actual, error; const float target = planner.get_axis_position_mm(encoderAxis),
actual = mm_from_count(position),
target = planner.get_axis_position_mm(encoderAxis); diff = actual - target,
actual = mm_from_count(position); error = ABS(diff) > 10000 ? 0 : diff; // Huge error is a bad reading
error = actual - target;
if (ABS(error) > 10000) error = 0; // ?
if (report) { if (report) {
SERIAL_ECHO(axis_codes[encoderAxis]); 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; return error;
@ -278,21 +278,25 @@ int32_t I2CPositionEncoder::get_axis_error_steps(const bool report) {
//convert both 'ticks' into same units / base //convert both 'ticks' into same units / base
encoderCountInStepperTicksScaled = LROUND((stepperTicksPerUnit * encoderTicks) / encoderTicksPerUnit); encoderCountInStepperTicksScaled = LROUND((stepperTicksPerUnit * encoderTicks) / encoderTicksPerUnit);
int32_t target = stepper.position(encoderAxis), const int32_t target = stepper.position(encoderAxis);
error = (encoderCountInStepperTicksScaled - target); int32_t error = encoderCountInStepperTicksScaled - target;
//suppress discontinuities (might be caused by bad I2C readings...?) //suppress discontinuities (might be caused by bad I2C readings...?)
const bool suppressOutput = (ABS(error - errorPrev) > 100); 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; 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() { int32_t I2CPositionEncoder::get_raw_count() {

View File

@ -1629,7 +1629,7 @@
auto debug_pt = [](PGM_P const pre, const xy_pos_t &pos, const float &zadd) { auto debug_pt = [](PGM_P const pre, const xy_pos_t &pos, const float &zadd) {
d_from(); serialprintPGM(pre); d_from(); serialprintPGM(pre);
DEBUG_ECHO_F(normed(pos, zadd), 6); 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("1st point: "), probe_pt[0], normal.z * z1);
debug_pt(PSTR("2nd point: "), probe_pt[1], normal.z * z2); 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); DEBUG_ECHOLNPAIR_F("0 : ", normed(safe_homing_xy, 0), 6);
d_from(); DEBUG_ECHOPGM("safe home with Z="); 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_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); DEBUG_ECHOLNPAIR_F(") = ", get_z_correction(safe_homing_xy), 6);
#endif #endif
} // DEBUGGING(LEVELING) } // DEBUGGING(LEVELING)