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) {
// 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() {

View file

@ -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)