diff --git a/Marlin/src/feature/I2CPositionEncoder.cpp b/Marlin/src/feature/I2CPositionEncoder.cpp index 84334812f..bf4c3676d 100644 --- a/Marlin/src/feature/I2CPositionEncoder.cpp +++ b/Marlin/src/feature/I2CPositionEncoder.cpp @@ -28,1105 +28,1104 @@ //todo: consider Marlin-optimized Wire library; i.e. MarlinWire, like MarlinSerial -#include "MarlinConfig.h" +#include "../inc/MarlinConfig.h" #if ENABLED(I2C_POSITION_ENCODERS) - #include "Marlin.h" - #include "temperature.h" - #include "stepper.h" - #include "I2CPositionEncoder.h" - #include "gcode.h" +#include "I2CPositionEncoder.h" - #include +#include "../module/temperature.h" +#include "../module/stepper.h" +#include "../gcode/parser.h" +#include - void I2CPositionEncoder::init(const uint8_t address, const AxisEnum axis) { - encoderAxis = axis; - i2cAddress = address; +void I2CPositionEncoder::init(const uint8_t address, const AxisEnum axis) { + encoderAxis = axis; + i2cAddress = address; - initialised++; + initialised++; - SERIAL_ECHOPAIR("Setting up encoder on ", axis_codes[encoderAxis]); - SERIAL_ECHOLNPAIR(" axis, addr = ", address); + SERIAL_ECHOPAIR("Setting up encoder on ", axis_codes[encoderAxis]); + SERIAL_ECHOLNPAIR(" axis, addr = ", address); - position = get_position(); + position = get_position(); +} + +void I2CPositionEncoder::update() { + if (!initialised || !homed || !active) return; //check encoder is set up and active + + position = get_position(); + + //we don't want to stop things just because the encoder missed a message, + //so we only care about responses that indicate bad magnetic strength + + if (!passes_test(false)) { //check encoder data is good + lastErrorTime = millis(); + /* + if (trusted) { //commented out as part of the note below + trusted = false; + SERIAL_ECHOPGM("Fault detected on "); + SERIAL_ECHO(axis_codes[encoderAxis]); + SERIAL_ECHOLNPGM(" axis encoder. Disengaging error correction until module is trusted again."); + } + */ + return; } - void I2CPositionEncoder::update() { - if (!initialised || !homed || !active) return; //check encoder is set up and active + if (!trusted) { + /** + * This is commented out because it introduces error and can cause bad print quality. + * + * This code is intended to manage situations where the encoder has reported bad magnetic strength. + * This indicates that the magnetic strip was too far away from the sensor to reliably track position. + * When this happens, this code resets the offset based on where the printer thinks it is. This has been + * shown to introduce errors in actual position which result in drifting prints and poor print quality. + * Perhaps a better method would be to disable correction on the axis with a problem, report it to the + * user via the status leds on the encoder module and prompt the user to re-home the axis at which point + * the encoder would be re-enabled. + */ - position = get_position(); + /* + // If the magnetic strength has been good for a certain time, start trusting the module again - //we don't want to stop things just because the encoder missed a message, - //so we only care about responses that indicate bad magnetic strength + if (millis() - lastErrorTime > I2CPE_TIME_TRUSTED) { + trusted = true; - if (!passes_test(false)) { //check encoder data is good - lastErrorTime = millis(); - /* - if (trusted) { //commented out as part of the note below - trusted = false; - SERIAL_ECHOPGM("Fault detected on "); + SERIAL_ECHOPGM("Untrusted encoder module on "); SERIAL_ECHO(axis_codes[encoderAxis]); - SERIAL_ECHOLNPGM(" axis encoder. Disengaging error correction until module is trusted again."); + SERIAL_ECHOLNPGM(" axis has been fault-free for set duration, reinstating error correction."); + + //the encoder likely lost its place when the error occured, so we'll reset and use the printer's + //idea of where it the axis is to re-initialise + float position = stepper.get_axis_position_mm(encoderAxis); + int32_t positionInTicks = position * get_ticks_unit(); + + //shift position from previous to current position + zeroOffset -= (positionInTicks - get_position()); + + #ifdef I2CPE_DEBUG + SERIAL_ECHOPGM("Current position is "); + SERIAL_ECHOLN(position); + + SERIAL_ECHOPGM("Position in encoder ticks is "); + SERIAL_ECHOLN(positionInTicks); + + SERIAL_ECHOPGM("New zero-offset of "); + SERIAL_ECHOLN(zeroOffset); + + SERIAL_ECHOPGM("New position reads as "); + SERIAL_ECHO(get_position()); + SERIAL_ECHOPGM("("); + SERIAL_ECHO(mm_from_count(get_position())); + SERIAL_ECHOLNPGM(")"); + #endif } - */ - return; - } + */ + return; + } - if (!trusted) { - /** - * This is commented out because it introduces error and can cause bad print quality. - * - * This code is intended to manage situations where the encoder has reported bad magnetic strength. - * This indicates that the magnetic strip was too far away from the sensor to reliably track position. - * When this happens, this code resets the offset based on where the printer thinks it is. This has been - * shown to introduce errors in actual position which result in drifting prints and poor print quality. - * Perhaps a better method would be to disable correction on the axis with a problem, report it to the - * user via the status leds on the encoder module and prompt the user to re-home the axis at which point - * the encoder would be re-enabled. - */ + lastPosition = position; + const millis_t positionTime = millis(); - /* - // If the magnetic strength has been good for a certain time, start trusting the module again + //only do error correction if setup and enabled + if (ec && ecMethod != I2CPE_ECM_NONE) { - if (millis() - lastErrorTime > I2CPE_TIME_TRUSTED) { - trusted = true; + #ifdef I2CPE_EC_THRESH_PROPORTIONAL + const millis_t deltaTime = positionTime - lastPositionTime; + const uint32_t distance = abs(position - lastPosition), + speed = distance / deltaTime; + const float threshold = constrain((speed / 50), 1, 50) * ecThreshold; + #else + const float threshold = get_error_correct_threshold(); + #endif - SERIAL_ECHOPGM("Untrusted encoder module on "); + //check error + #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE) + float sum = 0, diffSum = 0; + + errIdx = (errIdx >= I2CPE_ERR_ARRAY_SIZE - 1) ? 0 : errIdx + 1; + err[errIdx] = get_axis_error_steps(false); + + LOOP_L_N(i, I2CPE_ERR_ARRAY_SIZE) { + sum += err[i]; + if (i) diffSum += abs(err[i-1] - err[i]); + } + + const int32_t error = int32_t(sum / (I2CPE_ERR_ARRAY_SIZE + 1)); //calculate average for error + + #else + const int32_t error = get_axis_error_steps(false); + #endif + + //SERIAL_ECHOPGM("Axis error steps: "); + //SERIAL_ECHOLN(error); + + #ifdef I2CPE_ERR_THRESH_ABORT + if (labs(error) > I2CPE_ERR_THRESH_ABORT * planner.axis_steps_per_mm[encoderAxis]) { + //kill("Significant Error"); + SERIAL_ECHOPGM("Axis error greater than set threshold, aborting!"); + SERIAL_ECHOLN(error); + safe_delay(5000); + } + #endif + + #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE) + 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 (labs(error) > threshold * planner.axis_steps_per_mm[encoderAxis] && + diffSum < 10 * (I2CPE_ERR_ARRAY_SIZE - 1) && labs(error) < 2000) { //Check for persistent error (skip) SERIAL_ECHO(axis_codes[encoderAxis]); - SERIAL_ECHOLNPGM(" axis has been fault-free for set duration, reinstating error correction."); - - //the encoder likely lost its place when the error occured, so we'll reset and use the printer's - //idea of where it the axis is to re-initialise - float position = stepper.get_axis_position_mm(encoderAxis); - int32_t positionInTicks = position * get_ticks_unit(); - - //shift position from previous to current position - zeroOffset -= (positionInTicks - get_position()); - - #ifdef I2CPE_DEBUG - SERIAL_ECHOPGM("Current position is "); - SERIAL_ECHOLN(position); - - SERIAL_ECHOPGM("Position in encoder ticks is "); - SERIAL_ECHOLN(positionInTicks); - - SERIAL_ECHOPGM("New zero-offset of "); - SERIAL_ECHOLN(zeroOffset); - - SERIAL_ECHOPGM("New position reads as "); - SERIAL_ECHO(get_position()); - SERIAL_ECHOPGM("("); - SERIAL_ECHO(mm_from_count(get_position())); - SERIAL_ECHOLNPGM(")"); - #endif - } - */ - return; - } - - lastPosition = position; - const millis_t positionTime = millis(); - - //only do error correction if setup and enabled - if (ec && ecMethod != I2CPE_ECM_NONE) { - - #ifdef I2CPE_EC_THRESH_PROPORTIONAL - const millis_t deltaTime = positionTime - lastPositionTime; - const uint32_t distance = abs(position - lastPosition), - speed = distance / deltaTime; - const float threshold = constrain((speed / 50), 1, 50) * ecThreshold; - #else - const float threshold = get_error_correct_threshold(); - #endif - - //check error - #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE) - float sum = 0, diffSum = 0; - - errIdx = (errIdx >= I2CPE_ERR_ARRAY_SIZE - 1) ? 0 : errIdx + 1; - err[errIdx] = get_axis_error_steps(false); - - LOOP_L_N(i, I2CPE_ERR_ARRAY_SIZE) { - sum += err[i]; - if (i) diffSum += abs(err[i-1] - err[i]); - } - - const int32_t error = int32_t(sum / (I2CPE_ERR_ARRAY_SIZE + 1)); //calculate average for error - - #else - const int32_t error = get_axis_error_steps(false); - #endif - - //SERIAL_ECHOPGM("Axis error steps: "); - //SERIAL_ECHOLN(error); - - #ifdef I2CPE_ERR_THRESH_ABORT - if (labs(error) > I2CPE_ERR_THRESH_ABORT * planner.axis_steps_per_mm[encoderAxis]) { - //kill("Significant Error"); - SERIAL_ECHOPGM("Axis error greater than set threshold, aborting!"); - SERIAL_ECHOLN(error); - safe_delay(5000); - } - #endif - - #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE) - 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 (labs(error) > threshold * planner.axis_steps_per_mm[encoderAxis] && - diffSum < 10 * (I2CPE_ERR_ARRAY_SIZE - 1) && labs(error) < 2000) { //Check for persistent error (skip) - SERIAL_ECHO(axis_codes[encoderAxis]); - SERIAL_ECHOPAIR(" diffSum: ", diffSum / (I2CPE_ERR_ARRAY_SIZE - 1)); - SERIAL_ECHOPAIR(" - err detected: ", error / planner.axis_steps_per_mm[encoderAxis]); - SERIAL_ECHOLNPGM("mm; correcting!"); - thermalManager.babystepsTodo[encoderAxis] = -LROUND(error); - } - } - #else - if (labs(error) > threshold * planner.axis_steps_per_mm[encoderAxis]) { - //SERIAL_ECHOLN(error); - //SERIAL_ECHOLN(position); - thermalManager.babystepsTodo[encoderAxis] = -LROUND(error/2); - } - #endif - - if (labs(error) > I2CPE_ERR_CNT_THRESH * planner.axis_steps_per_mm[encoderAxis]) { - const millis_t ms = millis(); - if (ELAPSED(ms, nextErrorCountTime)) { - SERIAL_ECHOPAIR("Large error on ", axis_codes[encoderAxis]); - SERIAL_ECHOPAIR(" axis. error: ", (int)error); - SERIAL_ECHOLNPAIR("; diffSum: ", diffSum); - errorCount++; - nextErrorCountTime = ms + I2CPE_ERR_CNT_DEBOUNCE_MS; + SERIAL_ECHOPAIR(" diffSum: ", diffSum / (I2CPE_ERR_ARRAY_SIZE - 1)); + SERIAL_ECHOPAIR(" - err detected: ", error / planner.axis_steps_per_mm[encoderAxis]); + SERIAL_ECHOLNPGM("mm; correcting!"); + thermalManager.babystepsTodo[encoderAxis] = -LROUND(error); } } - } + #else + if (labs(error) > threshold * planner.axis_steps_per_mm[encoderAxis]) { + //SERIAL_ECHOLN(error); + //SERIAL_ECHOLN(position); + thermalManager.babystepsTodo[encoderAxis] = -LROUND(error/2); + } + #endif - lastPositionTime = positionTime; - } - - void I2CPositionEncoder::set_homed() { - if (active) { - reset(); // Reset module's offset to zero (so current position is homed / zero) - delay(10); - - zeroOffset = get_raw_count(); - homed++; - trusted++; - - #ifdef I2CPE_DEBUG - SERIAL_ECHO(axis_codes[encoderAxis]); - SERIAL_ECHOPAIR(" axis encoder homed, offset of ", zeroOffset); - SERIAL_ECHOLNPGM(" ticks."); - #endif - } - } - - bool I2CPositionEncoder::passes_test(const bool report) { - if (report) { - if (H != I2CPE_MAG_SIG_GOOD) SERIAL_ECHOPGM("Warning. "); - SERIAL_ECHO(axis_codes[encoderAxis]); - SERIAL_ECHOPGM(" axis "); - serialprintPGM(H == I2CPE_MAG_SIG_BAD ? PSTR("magnetic strip ") : PSTR("encoder ")); - switch (H) { - case I2CPE_MAG_SIG_GOOD: - case I2CPE_MAG_SIG_MID: - SERIAL_ECHOLNPGM("passes test; field strength "); - serialprintPGM(H == I2CPE_MAG_SIG_GOOD ? PSTR("good.\n") : PSTR("fair.\n")); - break; - default: - SERIAL_ECHOLNPGM("not detected!"); + if (labs(error) > I2CPE_ERR_CNT_THRESH * planner.axis_steps_per_mm[encoderAxis]) { + const millis_t ms = millis(); + if (ELAPSED(ms, nextErrorCountTime)) { + SERIAL_ECHOPAIR("Large error on ", axis_codes[encoderAxis]); + SERIAL_ECHOPAIR(" axis. error: ", (int)error); + SERIAL_ECHOLNPAIR("; diffSum: ", diffSum); + errorCount++; + nextErrorCountTime = ms + I2CPE_ERR_CNT_DEBOUNCE_MS; } } - return (H == I2CPE_MAG_SIG_GOOD || H == I2CPE_MAG_SIG_MID); } - float I2CPositionEncoder::get_axis_error_mm(const bool report) { - float target, actual, error; + lastPositionTime = positionTime; +} - target = stepper.get_axis_position_mm(encoderAxis); - actual = mm_from_count(position); - error = actual - target; +void I2CPositionEncoder::set_homed() { + if (active) { + reset(); // Reset module's offset to zero (so current position is homed / zero) + delay(10); - if (labs(error) > 10000) error = 0; // ? + zeroOffset = get_raw_count(); + homed++; + trusted++; + #ifdef I2CPE_DEBUG + SERIAL_ECHO(axis_codes[encoderAxis]); + SERIAL_ECHOPAIR(" axis encoder homed, offset of ", zeroOffset); + SERIAL_ECHOLNPGM(" ticks."); + #endif + } +} + +bool I2CPositionEncoder::passes_test(const bool report) { + if (report) { + if (H != I2CPE_MAG_SIG_GOOD) SERIAL_ECHOPGM("Warning. "); + SERIAL_ECHO(axis_codes[encoderAxis]); + SERIAL_ECHOPGM(" axis "); + serialprintPGM(H == I2CPE_MAG_SIG_BAD ? PSTR("magnetic strip ") : PSTR("encoder ")); + switch (H) { + case I2CPE_MAG_SIG_GOOD: + case I2CPE_MAG_SIG_MID: + SERIAL_ECHOLNPGM("passes test; field strength "); + serialprintPGM(H == I2CPE_MAG_SIG_GOOD ? PSTR("good.\n") : PSTR("fair.\n")); + break; + default: + SERIAL_ECHOLNPGM("not detected!"); + } + } + return (H == I2CPE_MAG_SIG_GOOD || H == I2CPE_MAG_SIG_MID); +} + +float I2CPositionEncoder::get_axis_error_mm(const bool report) { + float target, actual, error; + + target = stepper.get_axis_position_mm(encoderAxis); + actual = mm_from_count(position); + error = actual - target; + + if (labs(error) > 10000) error = 0; // ? + + if (report) { + SERIAL_ECHO(axis_codes[encoderAxis]); + SERIAL_ECHOPAIR(" axis target: ", target); + SERIAL_ECHOPAIR(", actual: ", actual); + SERIAL_ECHOLNPAIR(", error : ",error); + } + + return error; +} + +int32_t I2CPositionEncoder::get_axis_error_steps(const bool report) { + if (!active) { if (report) { SERIAL_ECHO(axis_codes[encoderAxis]); - SERIAL_ECHOPAIR(" axis target: ", target); - SERIAL_ECHOPAIR(", actual: ", actual); - SERIAL_ECHOLNPAIR(", error : ",error); + SERIAL_ECHOLNPGM(" axis encoder not active!"); } - - return error; + return 0; } - int32_t I2CPositionEncoder::get_axis_error_steps(const bool report) { - if (!active) { - if (report) { - SERIAL_ECHO(axis_codes[encoderAxis]); - SERIAL_ECHOLNPGM(" axis encoder not active!"); - } - return 0; - } + float stepperTicksPerUnit; + int32_t encoderTicks = position, encoderCountInStepperTicksScaled; + //int32_t stepperTicks = stepper.position(encoderAxis); - float stepperTicksPerUnit; - int32_t encoderTicks = position, encoderCountInStepperTicksScaled; - //int32_t stepperTicks = stepper.position(encoderAxis); + // With a rotary encoder we're concerned with ticks/rev; whereas with a linear we're concerned with ticks/mm + stepperTicksPerUnit = (type == I2CPE_ENC_TYPE_ROTARY) ? stepperTicks : planner.axis_steps_per_mm[encoderAxis]; - // With a rotary encoder we're concerned with ticks/rev; whereas with a linear we're concerned with ticks/mm - stepperTicksPerUnit = (type == I2CPE_ENC_TYPE_ROTARY) ? stepperTicks : planner.axis_steps_per_mm[encoderAxis]; + //convert both 'ticks' into same units / base + encoderCountInStepperTicksScaled = LROUND((stepperTicksPerUnit * encoderTicks) / encoderTicksPerUnit); - //convert both 'ticks' into same units / base - encoderCountInStepperTicksScaled = LROUND((stepperTicksPerUnit * encoderTicks) / encoderTicksPerUnit); + int32_t target = stepper.position(encoderAxis), + error = (encoderCountInStepperTicksScaled - target); - int32_t target = stepper.position(encoderAxis), - error = (encoderCountInStepperTicksScaled - target); + //suppress discontinuities (might be caused by bad I2C readings...?) + bool suppressOutput = (labs(error - errorPrev) > 100); - //suppress discontinuities (might be caused by bad I2C readings...?) - bool suppressOutput = (labs(error - errorPrev) > 100); + if (report) { + SERIAL_ECHO(axis_codes[encoderAxis]); + SERIAL_ECHOPAIR(" axis target: ", target); + SERIAL_ECHOPAIR(", actual: ", encoderCountInStepperTicksScaled); + SERIAL_ECHOLNPAIR(", error : ", error); - if (report) { - SERIAL_ECHO(axis_codes[encoderAxis]); - SERIAL_ECHOPAIR(" axis target: ", target); - SERIAL_ECHOPAIR(", actual: ", encoderCountInStepperTicksScaled); - SERIAL_ECHOLNPAIR(", error : ", error); - - if (suppressOutput) SERIAL_ECHOLNPGM("Discontinuity detected, suppressing error."); - } - - errorPrev = error; - - return (suppressOutput ? 0 : error); + if (suppressOutput) SERIAL_ECHOLNPGM("Discontinuity detected, suppressing error."); } - int32_t I2CPositionEncoder::get_raw_count() { - uint8_t index = 0; - i2cLong encoderCount; + errorPrev = error; - encoderCount.val = 0x00; + return (suppressOutput ? 0 : error); +} - if (Wire.requestFrom((int)i2cAddress, 3) != 3) { - //houston, we have a problem... - H = I2CPE_MAG_SIG_NF; - return 0; - } +int32_t I2CPositionEncoder::get_raw_count() { + uint8_t index = 0; + i2cLong encoderCount; - while (Wire.available()) - encoderCount.bval[index++] = (uint8_t)Wire.read(); + encoderCount.val = 0x00; - //extract the magnetic strength - H = (B00000011 & (encoderCount.bval[2] >> 6)); - - //extract sign bit; sign = (encoderCount.bval[2] & B00100000); - //set all upper bits to the sign value to overwrite H - encoderCount.val = (encoderCount.bval[2] & B00100000) ? (encoderCount.val | 0xFFC00000) : (encoderCount.val & 0x003FFFFF); - - if (invert) encoderCount.val *= -1; - - return encoderCount.val; + if (Wire.requestFrom((int)i2cAddress, 3) != 3) { + //houston, we have a problem... + H = I2CPE_MAG_SIG_NF; + return 0; } - bool I2CPositionEncoder::test_axis() { - //only works on XYZ cartesian machines for the time being - if (!(encoderAxis == X_AXIS || encoderAxis == Y_AXIS || encoderAxis == Z_AXIS)) return false; + while (Wire.available()) + encoderCount.bval[index++] = (uint8_t)Wire.read(); - float startCoord[NUM_AXIS] = { 0 }, endCoord[NUM_AXIS] = { 0 }; + //extract the magnetic strength + H = (B00000011 & (encoderCount.bval[2] >> 6)); - const float startPosition = soft_endstop_min[encoderAxis] + 10, - endPosition = soft_endstop_max[encoderAxis] - 10, - feedrate = FLOOR(MMM_TO_MMS((encoderAxis == Z_AXIS) ? HOMING_FEEDRATE_Z : HOMING_FEEDRATE_XY)); + //extract sign bit; sign = (encoderCount.bval[2] & B00100000); + //set all upper bits to the sign value to overwrite H + encoderCount.val = (encoderCount.bval[2] & B00100000) ? (encoderCount.val | 0xFFC00000) : (encoderCount.val & 0x003FFFFF); - ec = false; + if (invert) encoderCount.val *= -1; - LOOP_NA(i) { - startCoord[i] = stepper.get_axis_position_mm((AxisEnum)i); - endCoord[i] = stepper.get_axis_position_mm((AxisEnum)i); - } + return encoderCount.val; +} - startCoord[encoderAxis] = startPosition; - endCoord[encoderAxis] = endPosition; +bool I2CPositionEncoder::test_axis() { + //only works on XYZ cartesian machines for the time being + if (!(encoderAxis == X_AXIS || encoderAxis == Y_AXIS || encoderAxis == Z_AXIS)) return false; + float startCoord[NUM_AXIS] = { 0 }, endCoord[NUM_AXIS] = { 0 }; + + const float startPosition = soft_endstop_min[encoderAxis] + 10, + endPosition = soft_endstop_max[encoderAxis] - 10, + feedrate = FLOOR(MMM_TO_MMS((encoderAxis == Z_AXIS) ? HOMING_FEEDRATE_Z : HOMING_FEEDRATE_XY)); + + ec = false; + + LOOP_NA(i) { + startCoord[i] = stepper.get_axis_position_mm((AxisEnum)i); + endCoord[i] = stepper.get_axis_position_mm((AxisEnum)i); + } + + startCoord[encoderAxis] = startPosition; + endCoord[encoderAxis] = endPosition; + + stepper.synchronize(); + + planner.buffer_line(startCoord[X_AXIS],startCoord[Y_AXIS],startCoord[Z_AXIS], + stepper.get_axis_position_mm(E_AXIS), feedrate, 0); + stepper.synchronize(); + + // if the module isn't currently trusted, wait until it is (or until it should be if things are working) + if (!trusted) { + int32_t startWaitingTime = millis(); + while (!trusted && millis() - startWaitingTime < I2CPE_TIME_TRUSTED) + safe_delay(500); + } + + if (trusted) { // if trusted, commence test + planner.buffer_line(endCoord[X_AXIS], endCoord[Y_AXIS], endCoord[Z_AXIS], + stepper.get_axis_position_mm(E_AXIS), feedrate, 0); + stepper.synchronize(); + } + + return trusted; +} + +void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) { + if (type != I2CPE_ENC_TYPE_LINEAR) { + SERIAL_ECHOLNPGM("Steps per mm calibration is only available using linear encoders."); + return; + } + + if (!(encoderAxis == X_AXIS || encoderAxis == Y_AXIS || encoderAxis == Z_AXIS)) { + SERIAL_ECHOLNPGM("Automatic steps / mm calibration not supported for this axis."); + return; + } + + float old_steps_mm, new_steps_mm, + startDistance, endDistance, + travelDistance, travelledDistance, total = 0, + startCoord[NUM_AXIS] = { 0 }, endCoord[NUM_AXIS] = { 0 }; + + float feedrate; + + int32_t startCount, stopCount; + + feedrate = MMM_TO_MMS((encoderAxis == Z_AXIS) ? HOMING_FEEDRATE_Z : HOMING_FEEDRATE_XY); + + bool oldec = ec; + ec = false; + + startDistance = 20; + endDistance = soft_endstop_max[encoderAxis] - 20; + travelDistance = endDistance - startDistance; + + LOOP_NA(i) { + startCoord[i] = stepper.get_axis_position_mm((AxisEnum)i); + endCoord[i] = stepper.get_axis_position_mm((AxisEnum)i); + } + + startCoord[encoderAxis] = startDistance; + endCoord[encoderAxis] = endDistance; + + LOOP_L_N(i, iter) { stepper.synchronize(); planner.buffer_line(startCoord[X_AXIS],startCoord[Y_AXIS],startCoord[Z_AXIS], stepper.get_axis_position_mm(E_AXIS), feedrate, 0); stepper.synchronize(); - // if the module isn't currently trusted, wait until it is (or until it should be if things are working) - if (!trusted) { - int32_t startWaitingTime = millis(); - while (!trusted && millis() - startWaitingTime < I2CPE_TIME_TRUSTED) - safe_delay(500); - } + delay(250); + startCount = get_position(); - if (trusted) { // if trusted, commence test - planner.buffer_line(endCoord[X_AXIS], endCoord[Y_AXIS], endCoord[Z_AXIS], - stepper.get_axis_position_mm(E_AXIS), feedrate, 0); - stepper.synchronize(); - } + //do_blocking_move_to(endCoord[X_AXIS],endCoord[Y_AXIS],endCoord[Z_AXIS]); - return trusted; - } + planner.buffer_line(endCoord[X_AXIS],endCoord[Y_AXIS],endCoord[Z_AXIS], + stepper.get_axis_position_mm(E_AXIS), feedrate, 0); + stepper.synchronize(); - void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) { - if (type != I2CPE_ENC_TYPE_LINEAR) { - SERIAL_ECHOLNPGM("Steps per mm calibration is only available using linear encoders."); - return; - } + //Read encoder distance + delay(250); + stopCount = get_position(); - if (!(encoderAxis == X_AXIS || encoderAxis == Y_AXIS || encoderAxis == Z_AXIS)) { - SERIAL_ECHOLNPGM("Automatic steps / mm calibration not supported for this axis."); - return; - } + travelledDistance = mm_from_count(abs(stopCount - startCount)); - float old_steps_mm, new_steps_mm, - startDistance, endDistance, - travelDistance, travelledDistance, total = 0, - startCoord[NUM_AXIS] = { 0 }, endCoord[NUM_AXIS] = { 0 }; + SERIAL_ECHOPAIR("Attempted to travel: ", travelDistance); + SERIAL_ECHOLNPGM("mm."); - float feedrate; + SERIAL_ECHOPAIR("Actually travelled: ", travelledDistance); + SERIAL_ECHOLNPGM("mm."); - int32_t startCount, stopCount; + //Calculate new axis steps per unit + old_steps_mm = planner.axis_steps_per_mm[encoderAxis]; + new_steps_mm = (old_steps_mm * travelDistance) / travelledDistance; - feedrate = MMM_TO_MMS((encoderAxis == Z_AXIS) ? HOMING_FEEDRATE_Z : HOMING_FEEDRATE_XY); + SERIAL_ECHOLNPAIR("Old steps per mm: ", old_steps_mm); + SERIAL_ECHOLNPAIR("New steps per mm: ", new_steps_mm); - bool oldec = ec; - ec = false; - - startDistance = 20; - endDistance = soft_endstop_max[encoderAxis] - 20; - travelDistance = endDistance - startDistance; - - LOOP_NA(i) { - startCoord[i] = stepper.get_axis_position_mm((AxisEnum)i); - endCoord[i] = stepper.get_axis_position_mm((AxisEnum)i); - } - - startCoord[encoderAxis] = startDistance; - endCoord[encoderAxis] = endDistance; - - LOOP_L_N(i, iter) { - stepper.synchronize(); - - planner.buffer_line(startCoord[X_AXIS],startCoord[Y_AXIS],startCoord[Z_AXIS], - stepper.get_axis_position_mm(E_AXIS), feedrate, 0); - stepper.synchronize(); - - delay(250); - startCount = get_position(); - - //do_blocking_move_to(endCoord[X_AXIS],endCoord[Y_AXIS],endCoord[Z_AXIS]); - - planner.buffer_line(endCoord[X_AXIS],endCoord[Y_AXIS],endCoord[Z_AXIS], - stepper.get_axis_position_mm(E_AXIS), feedrate, 0); - stepper.synchronize(); - - //Read encoder distance - delay(250); - stopCount = get_position(); - - travelledDistance = mm_from_count(abs(stopCount - startCount)); - - SERIAL_ECHOPAIR("Attempted to travel: ", travelDistance); - SERIAL_ECHOLNPGM("mm."); - - SERIAL_ECHOPAIR("Actually travelled: ", travelledDistance); - SERIAL_ECHOLNPGM("mm."); - - //Calculate new axis steps per unit - old_steps_mm = planner.axis_steps_per_mm[encoderAxis]; - new_steps_mm = (old_steps_mm * travelDistance) / travelledDistance; - - SERIAL_ECHOLNPAIR("Old steps per mm: ", old_steps_mm); - SERIAL_ECHOLNPAIR("New steps per mm: ", new_steps_mm); - - //Save new value - planner.axis_steps_per_mm[encoderAxis] = new_steps_mm; - - if (iter > 1) { - total += new_steps_mm; - - // swap start and end points so next loop runs from current position - float tempCoord = startCoord[encoderAxis]; - startCoord[encoderAxis] = endCoord[encoderAxis]; - endCoord[encoderAxis] = tempCoord; - } - } + //Save new value + planner.axis_steps_per_mm[encoderAxis] = new_steps_mm; if (iter > 1) { - total /= (float)iter; - SERIAL_ECHOLNPAIR("Average steps per mm: ", total); + total += new_steps_mm; + + // swap start and end points so next loop runs from current position + float tempCoord = startCoord[encoderAxis]; + startCoord[encoderAxis] = endCoord[encoderAxis]; + endCoord[encoderAxis] = tempCoord; } - - ec = oldec; - - SERIAL_ECHOLNPGM("Calculated steps per mm has been set. Please save to EEPROM (M500) if you wish to keep these values."); } - void I2CPositionEncoder::reset() { - Wire.beginTransmission(i2cAddress); - Wire.write(I2CPE_RESET_COUNT); - Wire.endTransmission(); - - #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE) - ZERO(err); - #endif + if (iter > 1) { + total /= (float)iter; + SERIAL_ECHOLNPAIR("Average steps per mm: ", total); } + ec = oldec; - bool I2CPositionEncodersMgr::I2CPE_anyaxis; - uint8_t I2CPositionEncodersMgr::I2CPE_addr, - I2CPositionEncodersMgr::I2CPE_idx; - I2CPositionEncoder I2CPositionEncodersMgr::encoders[I2CPE_ENCODER_CNT]; + SERIAL_ECHOLNPGM("Calculated steps per mm has been set. Please save to EEPROM (M500) if you wish to keep these values."); +} - void I2CPositionEncodersMgr::init() { - Wire.begin(); +void I2CPositionEncoder::reset() { + Wire.beginTransmission(i2cAddress); + Wire.write(I2CPE_RESET_COUNT); + Wire.endTransmission(); - #if I2CPE_ENCODER_CNT > 0 - uint8_t i = 0; + #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE) + ZERO(err); + #endif +} - encoders[i].init(I2CPE_ENC_1_ADDR, I2CPE_ENC_1_AXIS); - #ifdef I2CPE_ENC_1_TYPE - encoders[i].set_type(I2CPE_ENC_1_TYPE); - #endif - #ifdef I2CPE_ENC_1_TICKS_UNIT - encoders[i].set_ticks_unit(I2CPE_ENC_1_TICKS_UNIT); - #endif - #ifdef I2CPE_ENC_1_TICKS_REV - encoders[i].set_stepper_ticks(I2CPE_ENC_1_TICKS_REV); - #endif - #ifdef I2CPE_ENC_1_INVERT - encoders[i].set_inverted(I2CPE_ENC_1_INVERT); - #endif - #ifdef I2CPE_ENC_1_EC_METHOD - encoders[i].set_ec_method(I2CPE_ENC_1_EC_METHOD); - #endif - #ifdef I2CPE_ENC_1_EC_THRESH - encoders[i].set_ec_threshold(I2CPE_ENC_1_EC_THRESH); - #endif +bool I2CPositionEncodersMgr::I2CPE_anyaxis; +uint8_t I2CPositionEncodersMgr::I2CPE_addr, + I2CPositionEncodersMgr::I2CPE_idx; +I2CPositionEncoder I2CPositionEncodersMgr::encoders[I2CPE_ENCODER_CNT]; - encoders[i].set_active(encoders[i].passes_test(true)); +void I2CPositionEncodersMgr::init() { + Wire.begin(); - #if I2CPE_ENC_1_AXIS == E_AXIS - encoders[i].set_homed(); - #endif + #if I2CPE_ENCODER_CNT > 0 + uint8_t i = 0; + + encoders[i].init(I2CPE_ENC_1_ADDR, I2CPE_ENC_1_AXIS); + + #ifdef I2CPE_ENC_1_TYPE + encoders[i].set_type(I2CPE_ENC_1_TYPE); #endif - - #if I2CPE_ENCODER_CNT > 1 - i++; - - encoders[i].init(I2CPE_ENC_2_ADDR, I2CPE_ENC_2_AXIS); - - #ifdef I2CPE_ENC_2_TYPE - encoders[i].set_type(I2CPE_ENC_2_TYPE); - #endif - #ifdef I2CPE_ENC_2_TICKS_UNIT - encoders[i].set_ticks_unit(I2CPE_ENC_2_TICKS_UNIT); - #endif - #ifdef I2CPE_ENC_2_TICKS_REV - encoders[i].set_stepper_ticks(I2CPE_ENC_2_TICKS_REV); - #endif - #ifdef I2CPE_ENC_2_INVERT - encoders[i].set_inverted(I2CPE_ENC_2_INVERT); - #endif - #ifdef I2CPE_ENC_2_EC_METHOD - encoders[i].set_ec_method(I2CPE_ENC_2_EC_METHOD); - #endif - #ifdef I2CPE_ENC_2_EC_THRESH - encoders[i].set_ec_threshold(I2CPE_ENC_2_EC_THRESH); - #endif - - encoders[i].set_active(encoders[i].passes_test(true)); - - #if I2CPE_ENC_2_AXIS == E_AXIS - encoders[i].set_homed(); - #endif + #ifdef I2CPE_ENC_1_TICKS_UNIT + encoders[i].set_ticks_unit(I2CPE_ENC_1_TICKS_UNIT); + #endif + #ifdef I2CPE_ENC_1_TICKS_REV + encoders[i].set_stepper_ticks(I2CPE_ENC_1_TICKS_REV); + #endif + #ifdef I2CPE_ENC_1_INVERT + encoders[i].set_inverted(I2CPE_ENC_1_INVERT); + #endif + #ifdef I2CPE_ENC_1_EC_METHOD + encoders[i].set_ec_method(I2CPE_ENC_1_EC_METHOD); + #endif + #ifdef I2CPE_ENC_1_EC_THRESH + encoders[i].set_ec_threshold(I2CPE_ENC_1_EC_THRESH); #endif - - #if I2CPE_ENCODER_CNT > 2 - i++; - - encoders[i].init(I2CPE_ENC_3_ADDR, I2CPE_ENC_3_AXIS); - - #ifdef I2CPE_ENC_3_TYPE - encoders[i].set_type(I2CPE_ENC_3_TYPE); - #endif - #ifdef I2CPE_ENC_3_TICKS_UNIT - encoders[i].set_ticks_unit(I2CPE_ENC_3_TICKS_UNIT); - #endif - #ifdef I2CPE_ENC_3_TICKS_REV - encoders[i].set_stepper_ticks(I2CPE_ENC_3_TICKS_REV); - #endif - #ifdef I2CPE_ENC_3_INVERT - encoders[i].set_inverted(I2CPE_ENC_3_INVERT); - #endif - #ifdef I2CPE_ENC_3_EC_METHOD - encoders[i].set_ec_method(I2CPE_ENC_3_EC_METHOD); - #endif - #ifdef I2CPE_ENC_3_EC_THRESH - encoders[i].set_ec_threshold(I2CPE_ENC_3_EC_THRESH); - #endif encoders[i].set_active(encoders[i].passes_test(true)); - #if I2CPE_ENC_3_AXIS == E_AXIS - encoders[i].set_homed(); - #endif + #if I2CPE_ENC_1_AXIS == E_AXIS + encoders[i].set_homed(); + #endif + #endif + + #if I2CPE_ENCODER_CNT > 1 + i++; + + encoders[i].init(I2CPE_ENC_2_ADDR, I2CPE_ENC_2_AXIS); + + #ifdef I2CPE_ENC_2_TYPE + encoders[i].set_type(I2CPE_ENC_2_TYPE); + #endif + #ifdef I2CPE_ENC_2_TICKS_UNIT + encoders[i].set_ticks_unit(I2CPE_ENC_2_TICKS_UNIT); + #endif + #ifdef I2CPE_ENC_2_TICKS_REV + encoders[i].set_stepper_ticks(I2CPE_ENC_2_TICKS_REV); + #endif + #ifdef I2CPE_ENC_2_INVERT + encoders[i].set_inverted(I2CPE_ENC_2_INVERT); + #endif + #ifdef I2CPE_ENC_2_EC_METHOD + encoders[i].set_ec_method(I2CPE_ENC_2_EC_METHOD); + #endif + #ifdef I2CPE_ENC_2_EC_THRESH + encoders[i].set_ec_threshold(I2CPE_ENC_2_EC_THRESH); #endif - #if I2CPE_ENCODER_CNT > 3 - i++; + encoders[i].set_active(encoders[i].passes_test(true)); - encoders[i].init(I2CPE_ENC_4_ADDR, I2CPE_ENC_4_AXIS); + #if I2CPE_ENC_2_AXIS == E_AXIS + encoders[i].set_homed(); + #endif + #endif - #ifdef I2CPE_ENC_4_TYPE - encoders[i].set_type(I2CPE_ENC_4_TYPE); - #endif - #ifdef I2CPE_ENC_4_TICKS_UNIT - encoders[i].set_ticks_unit(I2CPE_ENC_4_TICKS_UNIT); - #endif - #ifdef I2CPE_ENC_4_TICKS_REV - encoders[i].set_stepper_ticks(I2CPE_ENC_4_TICKS_REV); - #endif - #ifdef I2CPE_ENC_4_INVERT - encoders[i].set_inverted(I2CPE_ENC_4_INVERT); - #endif - #ifdef I2CPE_ENC_4_EC_METHOD - encoders[i].set_ec_method(I2CPE_ENC_4_EC_METHOD); - #endif - #ifdef I2CPE_ENC_4_EC_THRESH - encoders[i].set_ec_threshold(I2CPE_ENC_4_EC_THRESH); - #endif + #if I2CPE_ENCODER_CNT > 2 + i++; - encoders[i].set_active(encoders[i].passes_test(true)); + encoders[i].init(I2CPE_ENC_3_ADDR, I2CPE_ENC_3_AXIS); - #if I2CPE_ENC_4_AXIS == E_AXIS - encoders[i].set_homed(); - #endif + #ifdef I2CPE_ENC_3_TYPE + encoders[i].set_type(I2CPE_ENC_3_TYPE); + #endif + #ifdef I2CPE_ENC_3_TICKS_UNIT + encoders[i].set_ticks_unit(I2CPE_ENC_3_TICKS_UNIT); + #endif + #ifdef I2CPE_ENC_3_TICKS_REV + encoders[i].set_stepper_ticks(I2CPE_ENC_3_TICKS_REV); + #endif + #ifdef I2CPE_ENC_3_INVERT + encoders[i].set_inverted(I2CPE_ENC_3_INVERT); + #endif + #ifdef I2CPE_ENC_3_EC_METHOD + encoders[i].set_ec_method(I2CPE_ENC_3_EC_METHOD); + #endif + #ifdef I2CPE_ENC_3_EC_THRESH + encoders[i].set_ec_threshold(I2CPE_ENC_3_EC_THRESH); #endif - #if I2CPE_ENCODER_CNT > 4 - i++; + encoders[i].set_active(encoders[i].passes_test(true)); - encoders[i].init(I2CPE_ENC_5_ADDR, I2CPE_ENC_5_AXIS); - - #ifdef I2CPE_ENC_5_TYPE - encoders[i].set_type(I2CPE_ENC_5_TYPE); - #endif - #ifdef I2CPE_ENC_5_TICKS_UNIT - encoders[i].set_ticks_unit(I2CPE_ENC_5_TICKS_UNIT); - #endif - #ifdef I2CPE_ENC_5_TICKS_REV - encoders[i].set_stepper_ticks(I2CPE_ENC_5_TICKS_REV); - #endif - #ifdef I2CPE_ENC_5_INVERT - encoders[i].set_inverted(I2CPE_ENC_5_INVERT); - #endif - #ifdef I2CPE_ENC_5_EC_METHOD - encoders[i].set_ec_method(I2CPE_ENC_5_EC_METHOD); - #endif - #ifdef I2CPE_ENC_5_EC_THRESH - encoders[i].set_ec_threshold(I2CPE_ENC_5_EC_THRESH); - #endif - - encoders[i].set_active(encoders[i].passes_test(true)); - - #if I2CPE_ENC_5_AXIS == E_AXIS - encoders[i].set_homed(); - #endif + #if I2CPE_ENC_3_AXIS == E_AXIS + encoders[i].set_homed(); #endif - } + #endif - void I2CPositionEncodersMgr::report_position(const int8_t idx, const bool units, const bool noOffset) { - CHECK_IDX(); + #if I2CPE_ENCODER_CNT > 3 + i++; - if (units) - SERIAL_ECHOLN(noOffset ? encoders[idx].mm_from_count(encoders[idx].get_raw_count()) : encoders[idx].get_position_mm()); - else { - if (noOffset) { - const int32_t raw_count = encoders[idx].get_raw_count(); - SERIAL_ECHO(axis_codes[encoders[idx].get_axis()]); - SERIAL_CHAR(' '); + encoders[i].init(I2CPE_ENC_4_ADDR, I2CPE_ENC_4_AXIS); - for (uint8_t j = 31; j > 0; j--) - SERIAL_ECHO((bool)(0x00000001 & (raw_count >> j))); + #ifdef I2CPE_ENC_4_TYPE + encoders[i].set_type(I2CPE_ENC_4_TYPE); + #endif + #ifdef I2CPE_ENC_4_TICKS_UNIT + encoders[i].set_ticks_unit(I2CPE_ENC_4_TICKS_UNIT); + #endif + #ifdef I2CPE_ENC_4_TICKS_REV + encoders[i].set_stepper_ticks(I2CPE_ENC_4_TICKS_REV); + #endif + #ifdef I2CPE_ENC_4_INVERT + encoders[i].set_inverted(I2CPE_ENC_4_INVERT); + #endif + #ifdef I2CPE_ENC_4_EC_METHOD + encoders[i].set_ec_method(I2CPE_ENC_4_EC_METHOD); + #endif + #ifdef I2CPE_ENC_4_EC_THRESH + encoders[i].set_ec_threshold(I2CPE_ENC_4_EC_THRESH); + #endif - SERIAL_ECHO((bool)(0x00000001 & raw_count)); - SERIAL_CHAR(' '); - SERIAL_ECHOLN(raw_count); - } - else - SERIAL_ECHOLN(encoders[idx].get_position()); - } - } + encoders[i].set_active(encoders[i].passes_test(true)); - void I2CPositionEncodersMgr::change_module_address(const uint8_t oldaddr, const uint8_t newaddr) { - // First check 'new' address is not in use - Wire.beginTransmission(newaddr); - if (!Wire.endTransmission()) { - SERIAL_ECHOPAIR("?There is already a device with that address on the I2C bus! (", newaddr); - SERIAL_ECHOLNPGM(")"); - return; - } + #if I2CPE_ENC_4_AXIS == E_AXIS + encoders[i].set_homed(); + #endif + #endif - // Now check that we can find the module on the oldaddr address - Wire.beginTransmission(oldaddr); - if (Wire.endTransmission()) { - SERIAL_ECHOPAIR("?No module detected at this address! (", oldaddr); - SERIAL_ECHOLNPGM(")"); - return; - } + #if I2CPE_ENCODER_CNT > 4 + i++; - SERIAL_ECHOPAIR("Module found at ", oldaddr); - SERIAL_ECHOLNPAIR(", changing address to ", newaddr); + encoders[i].init(I2CPE_ENC_5_ADDR, I2CPE_ENC_5_AXIS); - // Change the modules address - Wire.beginTransmission(oldaddr); - Wire.write(I2CPE_SET_ADDR); - Wire.write(newaddr); - Wire.endTransmission(); + #ifdef I2CPE_ENC_5_TYPE + encoders[i].set_type(I2CPE_ENC_5_TYPE); + #endif + #ifdef I2CPE_ENC_5_TICKS_UNIT + encoders[i].set_ticks_unit(I2CPE_ENC_5_TICKS_UNIT); + #endif + #ifdef I2CPE_ENC_5_TICKS_REV + encoders[i].set_stepper_ticks(I2CPE_ENC_5_TICKS_REV); + #endif + #ifdef I2CPE_ENC_5_INVERT + encoders[i].set_inverted(I2CPE_ENC_5_INVERT); + #endif + #ifdef I2CPE_ENC_5_EC_METHOD + encoders[i].set_ec_method(I2CPE_ENC_5_EC_METHOD); + #endif + #ifdef I2CPE_ENC_5_EC_THRESH + encoders[i].set_ec_threshold(I2CPE_ENC_5_EC_THRESH); + #endif - SERIAL_ECHOLNPGM("Address changed, resetting and waiting for confirmation.."); + encoders[i].set_active(encoders[i].passes_test(true)); - // Wait for the module to reset (can probably be improved by polling address with a timeout). - safe_delay(I2CPE_REBOOT_TIME); + #if I2CPE_ENC_5_AXIS == E_AXIS + encoders[i].set_homed(); + #endif + #endif +} - // Look for the module at the new address. - Wire.beginTransmission(newaddr); - if (Wire.endTransmission()) { - SERIAL_ECHOLNPGM("Address change failed! Check encoder module."); - return; - } +void I2CPositionEncodersMgr::report_position(const int8_t idx, const bool units, const bool noOffset) { + CHECK_IDX(); - SERIAL_ECHOLNPGM("Address change successful!"); - - // Now, if this module is configured, find which encoder instance it's supposed to correspond to - // and enable it (it will likely have failed initialisation on power-up, before the address change). - const int8_t idx = idx_from_addr(newaddr); - if (idx >= 0 && !encoders[idx].get_active()) { + if (units) + SERIAL_ECHOLN(noOffset ? encoders[idx].mm_from_count(encoders[idx].get_raw_count()) : encoders[idx].get_position_mm()); + else { + if (noOffset) { + const int32_t raw_count = encoders[idx].get_raw_count(); SERIAL_ECHO(axis_codes[encoders[idx].get_axis()]); - SERIAL_ECHOLNPGM(" axis encoder was not detected on printer startup. Trying again."); - encoders[idx].set_active(encoders[idx].passes_test(true)); + SERIAL_CHAR(' '); + + for (uint8_t j = 31; j > 0; j--) + SERIAL_ECHO((bool)(0x00000001 & (raw_count >> j))); + + SERIAL_ECHO((bool)(0x00000001 & raw_count)); + SERIAL_CHAR(' '); + SERIAL_ECHOLN(raw_count); } + else + SERIAL_ECHOLN(encoders[idx].get_position()); + } +} + +void I2CPositionEncodersMgr::change_module_address(const uint8_t oldaddr, const uint8_t newaddr) { + // First check 'new' address is not in use + Wire.beginTransmission(newaddr); + if (!Wire.endTransmission()) { + SERIAL_ECHOPAIR("?There is already a device with that address on the I2C bus! (", newaddr); + SERIAL_ECHOLNPGM(")"); + return; } - void I2CPositionEncodersMgr::report_module_firmware(const uint8_t address) { - // First check there is a module - Wire.beginTransmission(address); - if (Wire.endTransmission()) { - SERIAL_ECHOPAIR("?No module detected at this address! (", address); - SERIAL_ECHOLNPGM(")"); + // Now check that we can find the module on the oldaddr address + Wire.beginTransmission(oldaddr); + if (Wire.endTransmission()) { + SERIAL_ECHOPAIR("?No module detected at this address! (", oldaddr); + SERIAL_ECHOLNPGM(")"); + return; + } + + SERIAL_ECHOPAIR("Module found at ", oldaddr); + SERIAL_ECHOLNPAIR(", changing address to ", newaddr); + + // Change the modules address + Wire.beginTransmission(oldaddr); + Wire.write(I2CPE_SET_ADDR); + Wire.write(newaddr); + Wire.endTransmission(); + + SERIAL_ECHOLNPGM("Address changed, resetting and waiting for confirmation.."); + + // Wait for the module to reset (can probably be improved by polling address with a timeout). + safe_delay(I2CPE_REBOOT_TIME); + + // Look for the module at the new address. + Wire.beginTransmission(newaddr); + if (Wire.endTransmission()) { + SERIAL_ECHOLNPGM("Address change failed! Check encoder module."); + return; + } + + SERIAL_ECHOLNPGM("Address change successful!"); + + // Now, if this module is configured, find which encoder instance it's supposed to correspond to + // and enable it (it will likely have failed initialisation on power-up, before the address change). + const int8_t idx = idx_from_addr(newaddr); + if (idx >= 0 && !encoders[idx].get_active()) { + SERIAL_ECHO(axis_codes[encoders[idx].get_axis()]); + SERIAL_ECHOLNPGM(" axis encoder was not detected on printer startup. Trying again."); + encoders[idx].set_active(encoders[idx].passes_test(true)); + } +} + +void I2CPositionEncodersMgr::report_module_firmware(const uint8_t address) { + // First check there is a module + Wire.beginTransmission(address); + if (Wire.endTransmission()) { + SERIAL_ECHOPAIR("?No module detected at this address! (", address); + SERIAL_ECHOLNPGM(")"); + return; + } + + SERIAL_ECHOPAIR("Requesting version info from module at address ", address); + SERIAL_ECHOLNPGM(":"); + + Wire.beginTransmission(address); + Wire.write(I2CPE_SET_REPORT_MODE); + Wire.write(I2CPE_REPORT_VERSION); + Wire.endTransmission(); + + // Read value + if (Wire.requestFrom((int)address, 32)) { + char c; + while (Wire.available() > 0 && (c = (char)Wire.read()) > 0) + SERIAL_ECHO(c); + SERIAL_EOL(); + } + + // Set module back to normal (distance) mode + Wire.beginTransmission(address); + Wire.write(I2CPE_SET_REPORT_MODE); + Wire.write(I2CPE_REPORT_DISTANCE); + Wire.endTransmission(); +} + +int8_t I2CPositionEncodersMgr::parse() { + I2CPE_addr = 0; + + if (parser.seen('A')) { + + if (!parser.has_value()) { + SERIAL_PROTOCOLLNPGM("?A seen, but no address specified! [30-200]"); + return I2CPE_PARSE_ERR; + }; + + I2CPE_addr = parser.value_byte(); + if (!WITHIN(I2CPE_addr, 30, 200)) { // reserve the first 30 and last 55 + SERIAL_PROTOCOLLNPGM("?Address out of range. [30-200]"); + return I2CPE_PARSE_ERR; + } + + I2CPE_idx = idx_from_addr(I2CPE_addr); + if (I2CPE_idx >= I2CPE_ENCODER_CNT) { + SERIAL_PROTOCOLLNPGM("?No device with this address!"); + return I2CPE_PARSE_ERR; + } + } + else if (parser.seenval('I')) { + + if (!parser.has_value()) { + SERIAL_PROTOCOLLNPAIR("?I seen, but no index specified! [0-", I2CPE_ENCODER_CNT - 1); + SERIAL_PROTOCOLLNPGM("]"); + return I2CPE_PARSE_ERR; + }; + + I2CPE_idx = parser.value_byte(); + if (I2CPE_idx >= I2CPE_ENCODER_CNT) { + SERIAL_PROTOCOLLNPAIR("?Index out of range. [0-", I2CPE_ENCODER_CNT - 1); + SERIAL_ECHOLNPGM("]"); + return I2CPE_PARSE_ERR; + } + + I2CPE_addr = encoders[I2CPE_idx].get_address(); + } + else + I2CPE_idx = 0xFF; + + I2CPE_anyaxis = parser.seen_axis(); + + return I2CPE_PARSE_OK; +}; + +/** + * M860: Report the position(s) of position encoder module(s). + * + * A Module I2C address. [30, 200]. + * I Module index. [0, I2CPE_ENCODER_CNT - 1] + * O Include homed zero-offset in returned position. + * U Units in mm or raw step count. + * + * If A or I not specified: + * X Report on X axis encoder, if present. + * Y Report on Y axis encoder, if present. + * Z Report on Z axis encoder, if present. + * E Report on E axis encoder, if present. + * + */ +void I2CPositionEncodersMgr::M860() { + if (parse()) return; + + const bool hasU = parser.seen('U'), hasO = parser.seen('O'); + + if (I2CPE_idx == 0xFF) { + LOOP_XYZE(i) { + if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { + const uint8_t idx = idx_from_axis(AxisEnum(i)); + if ((int8_t)idx >= 0) report_position(idx, hasU, hasO); + } + } + } + else + report_position(I2CPE_idx, hasU, hasO); +} + +/** + * M861: Report the status of position encoder modules. + * + * A Module I2C address. [30, 200]. + * I Module index. [0, I2CPE_ENCODER_CNT - 1] + * + * If A or I not specified: + * X Report on X axis encoder, if present. + * Y Report on Y axis encoder, if present. + * Z Report on Z axis encoder, if present. + * E Report on E axis encoder, if present. + * + */ +void I2CPositionEncodersMgr::M861() { + if (parse()) return; + + if (I2CPE_idx == 0xFF) { + LOOP_XYZE(i) { + if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { + const uint8_t idx = idx_from_axis(AxisEnum(i)); + if ((int8_t)idx >= 0) report_status(idx); + } + } + } + else + report_status(I2CPE_idx); +} + +/** + * M862: Perform an axis continuity test for position encoder + * modules. + * + * A Module I2C address. [30, 200]. + * I Module index. [0, I2CPE_ENCODER_CNT - 1] + * + * If A or I not specified: + * X Report on X axis encoder, if present. + * Y Report on Y axis encoder, if present. + * Z Report on Z axis encoder, if present. + * E Report on E axis encoder, if present. + * + */ +void I2CPositionEncodersMgr::M862() { + if (parse()) return; + + if (I2CPE_idx == 0xFF) { + LOOP_XYZE(i) { + if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { + const uint8_t idx = idx_from_axis(AxisEnum(i)); + if ((int8_t)idx >= 0) test_axis(idx); + } + } + } + else + test_axis(I2CPE_idx); +} + +/** + * M863: Perform steps-per-mm calibration for + * position encoder modules. + * + * A Module I2C address. [30, 200]. + * I Module index. [0, I2CPE_ENCODER_CNT - 1] + * P Number of rePeats/iterations. + * + * If A or I not specified: + * X Report on X axis encoder, if present. + * Y Report on Y axis encoder, if present. + * Z Report on Z axis encoder, if present. + * E Report on E axis encoder, if present. + * + */ +void I2CPositionEncodersMgr::M863() { + if (parse()) return; + + const uint8_t iterations = constrain(parser.byteval('P', 1), 1, 10); + + if (I2CPE_idx == 0xFF) { + LOOP_XYZE(i) { + if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { + const uint8_t idx = idx_from_axis(AxisEnum(i)); + if ((int8_t)idx >= 0) calibrate_steps_mm(idx, iterations); + } + } + } + else + calibrate_steps_mm(I2CPE_idx, iterations); +} + +/** + * M864: Change position encoder module I2C address. + * + * A Module current/old I2C address. If not present, + * assumes default address (030). [30, 200]. + * S Module new I2C address. [30, 200]. + * + * If S is not specified: + * X Use I2CPE_PRESET_ADDR_X (030). + * Y Use I2CPE_PRESET_ADDR_Y (031). + * Z Use I2CPE_PRESET_ADDR_Z (032). + * E Use I2CPE_PRESET_ADDR_E (033). + */ +void I2CPositionEncodersMgr::M864() { + uint8_t newAddress; + + if (parse()) return; + + if (!I2CPE_addr) I2CPE_addr = I2CPE_PRESET_ADDR_X; + + if (parser.seen('S')) { + if (!parser.has_value()) { + SERIAL_PROTOCOLLNPGM("?S seen, but no address specified! [30-200]"); + return; + }; + + newAddress = parser.value_byte(); + if (!WITHIN(newAddress, 30, 200)) { + SERIAL_PROTOCOLLNPGM("?New address out of range. [30-200]"); return; } - - SERIAL_ECHOPAIR("Requesting version info from module at address ", address); - SERIAL_ECHOLNPGM(":"); - - Wire.beginTransmission(address); - Wire.write(I2CPE_SET_REPORT_MODE); - Wire.write(I2CPE_REPORT_VERSION); - Wire.endTransmission(); - - // Read value - if (Wire.requestFrom((int)address, 32)) { - char c; - while (Wire.available() > 0 && (c = (char)Wire.read()) > 0) - SERIAL_ECHO(c); - SERIAL_EOL(); - } - - // Set module back to normal (distance) mode - Wire.beginTransmission(address); - Wire.write(I2CPE_SET_REPORT_MODE); - Wire.write(I2CPE_REPORT_DISTANCE); - Wire.endTransmission(); + } + else if (!I2CPE_anyaxis) { + SERIAL_PROTOCOLLNPGM("?You must specify S or [XYZE]."); + return; + } + else { + if (parser.seen('X')) newAddress = I2CPE_PRESET_ADDR_X; + else if (parser.seen('Y')) newAddress = I2CPE_PRESET_ADDR_Y; + else if (parser.seen('Z')) newAddress = I2CPE_PRESET_ADDR_Z; + else if (parser.seen('E')) newAddress = I2CPE_PRESET_ADDR_E; + else return; } - int8_t I2CPositionEncodersMgr::parse() { - I2CPE_addr = 0; + SERIAL_ECHOPAIR("Changing module at address ", I2CPE_addr); + SERIAL_ECHOLNPAIR(" to address ", newAddress); - if (parser.seen('A')) { + change_module_address(I2CPE_addr, newAddress); +} - if (!parser.has_value()) { - SERIAL_PROTOCOLLNPGM("?A seen, but no address specified! [30-200]"); - return I2CPE_PARSE_ERR; - }; +/** + * M865: Check position encoder module firmware version. + * + * A Module I2C address. [30, 200]. + * I Module index. [0, I2CPE_ENCODER_CNT - 1]. + * + * If A or I not specified: + * X Check X axis encoder, if present. + * Y Check Y axis encoder, if present. + * Z Check Z axis encoder, if present. + * E Check E axis encoder, if present. + */ +void I2CPositionEncodersMgr::M865() { + if (parse()) return; - I2CPE_addr = parser.value_byte(); - if (!WITHIN(I2CPE_addr, 30, 200)) { // reserve the first 30 and last 55 - SERIAL_PROTOCOLLNPGM("?Address out of range. [30-200]"); - return I2CPE_PARSE_ERR; - } - - I2CPE_idx = idx_from_addr(I2CPE_addr); - if (I2CPE_idx >= I2CPE_ENCODER_CNT) { - SERIAL_PROTOCOLLNPGM("?No device with this address!"); - return I2CPE_PARSE_ERR; + if (!I2CPE_addr) { + LOOP_XYZE(i) { + if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { + const uint8_t idx = idx_from_axis(AxisEnum(i)); + if ((int8_t)idx >= 0) report_module_firmware(encoders[idx].get_address()); } } - else if (parser.seenval('I')) { + } + else + report_module_firmware(I2CPE_addr); +} - if (!parser.has_value()) { - SERIAL_PROTOCOLLNPAIR("?I seen, but no index specified! [0-", I2CPE_ENCODER_CNT - 1); - SERIAL_PROTOCOLLNPGM("]"); - return I2CPE_PARSE_ERR; - }; +/** + * M866: Report or reset position encoder module error + * count. + * + * A Module I2C address. [30, 200]. + * I Module index. [0, I2CPE_ENCODER_CNT - 1]. + * R Reset error counter. + * + * If A or I not specified: + * X Act on X axis encoder, if present. + * Y Act on Y axis encoder, if present. + * Z Act on Z axis encoder, if present. + * E Act on E axis encoder, if present. + */ +void I2CPositionEncodersMgr::M866() { + if (parse()) return; - I2CPE_idx = parser.value_byte(); - if (I2CPE_idx >= I2CPE_ENCODER_CNT) { - SERIAL_PROTOCOLLNPAIR("?Index out of range. [0-", I2CPE_ENCODER_CNT - 1); - SERIAL_ECHOLNPGM("]"); - return I2CPE_PARSE_ERR; - } + const bool hasR = parser.seen('R'); - I2CPE_addr = encoders[I2CPE_idx].get_address(); - } - else - I2CPE_idx = 0xFF; - - I2CPE_anyaxis = parser.seen_axis(); - - return I2CPE_PARSE_OK; - }; - - /** - * M860: Report the position(s) of position encoder module(s). - * - * A Module I2C address. [30, 200]. - * I Module index. [0, I2CPE_ENCODER_CNT - 1] - * O Include homed zero-offset in returned position. - * U Units in mm or raw step count. - * - * If A or I not specified: - * X Report on X axis encoder, if present. - * Y Report on Y axis encoder, if present. - * Z Report on Z axis encoder, if present. - * E Report on E axis encoder, if present. - * - */ - void I2CPositionEncodersMgr::M860() { - if (parse()) return; - - const bool hasU = parser.seen('U'), hasO = parser.seen('O'); - - if (I2CPE_idx == 0xFF) { - LOOP_XYZE(i) { - if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { - const uint8_t idx = idx_from_axis(AxisEnum(i)); - if ((int8_t)idx >= 0) report_position(idx, hasU, hasO); + if (I2CPE_idx == 0xFF) { + LOOP_XYZE(i) { + if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { + const uint8_t idx = idx_from_axis(AxisEnum(i)); + if ((int8_t)idx >= 0) { + if (hasR) + reset_error_count(idx, AxisEnum(i)); + else + report_error_count(idx, AxisEnum(i)); } } } - else - report_position(I2CPE_idx, hasU, hasO); } + else if (hasR) + reset_error_count(I2CPE_idx, encoders[I2CPE_idx].get_axis()); + else + report_error_count(I2CPE_idx, encoders[I2CPE_idx].get_axis()); +} - /** - * M861: Report the status of position encoder modules. - * - * A Module I2C address. [30, 200]. - * I Module index. [0, I2CPE_ENCODER_CNT - 1] - * - * If A or I not specified: - * X Report on X axis encoder, if present. - * Y Report on Y axis encoder, if present. - * Z Report on Z axis encoder, if present. - * E Report on E axis encoder, if present. - * - */ - void I2CPositionEncodersMgr::M861() { - if (parse()) return; +/** + * M867: Enable/disable or toggle error correction for position encoder modules. + * + * A Module I2C address. [30, 200]. + * I Module index. [0, I2CPE_ENCODER_CNT - 1]. + * S<1|0> Enable/disable error correction. 1 enables, 0 disables. If not + * supplied, toggle. + * + * If A or I not specified: + * X Act on X axis encoder, if present. + * Y Act on Y axis encoder, if present. + * Z Act on Z axis encoder, if present. + * E Act on E axis encoder, if present. + */ +void I2CPositionEncodersMgr::M867() { + if (parse()) return; - if (I2CPE_idx == 0xFF) { - LOOP_XYZE(i) { - if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { - const uint8_t idx = idx_from_axis(AxisEnum(i)); - if ((int8_t)idx >= 0) report_status(idx); + const int8_t onoff = parser.seenval('S') ? parser.value_int() : -1; + + if (I2CPE_idx == 0xFF) { + LOOP_XYZE(i) { + if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { + const uint8_t idx = idx_from_axis(AxisEnum(i)); + if ((int8_t)idx >= 0) { + const bool ena = onoff == -1 ? !encoders[I2CPE_idx].get_ec_enabled() : !!onoff; + enable_ec(idx, ena, AxisEnum(i)); } } } - else - report_status(I2CPE_idx); } + else { + const bool ena = onoff == -1 ? !encoders[I2CPE_idx].get_ec_enabled() : !!onoff; + enable_ec(I2CPE_idx, ena, encoders[I2CPE_idx].get_axis()); + } +} - /** - * M862: Perform an axis continuity test for position encoder - * modules. - * - * A Module I2C address. [30, 200]. - * I Module index. [0, I2CPE_ENCODER_CNT - 1] - * - * If A or I not specified: - * X Report on X axis encoder, if present. - * Y Report on Y axis encoder, if present. - * Z Report on Z axis encoder, if present. - * E Report on E axis encoder, if present. - * - */ - void I2CPositionEncodersMgr::M862() { - if (parse()) return; +/** + * M868: Report or set position encoder module error correction + * threshold. + * + * A Module I2C address. [30, 200]. + * I Module index. [0, I2CPE_ENCODER_CNT - 1]. + * T New error correction threshold. + * + * If A not specified: + * X Act on X axis encoder, if present. + * Y Act on Y axis encoder, if present. + * Z Act on Z axis encoder, if present. + * E Act on E axis encoder, if present. + */ +void I2CPositionEncodersMgr::M868() { + if (parse()) return; - if (I2CPE_idx == 0xFF) { - LOOP_XYZE(i) { - if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { - const uint8_t idx = idx_from_axis(AxisEnum(i)); - if ((int8_t)idx >= 0) test_axis(idx); + const float newThreshold = parser.seenval('T') ? parser.value_float() : -9999; + + if (I2CPE_idx == 0xFF) { + LOOP_XYZE(i) { + if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { + const uint8_t idx = idx_from_axis(AxisEnum(i)); + if ((int8_t)idx >= 0) { + if (newThreshold != -9999) + set_ec_threshold(idx, newThreshold, encoders[idx].get_axis()); + else + get_ec_threshold(idx, encoders[idx].get_axis()); } } } - else - test_axis(I2CPE_idx); } + else if (newThreshold != -9999) + set_ec_threshold(I2CPE_idx, newThreshold, encoders[I2CPE_idx].get_axis()); + else + get_ec_threshold(I2CPE_idx, encoders[I2CPE_idx].get_axis()); +} - /** - * M863: Perform steps-per-mm calibration for - * position encoder modules. - * - * A Module I2C address. [30, 200]. - * I Module index. [0, I2CPE_ENCODER_CNT - 1] - * P Number of rePeats/iterations. - * - * If A or I not specified: - * X Report on X axis encoder, if present. - * Y Report on Y axis encoder, if present. - * Z Report on Z axis encoder, if present. - * E Report on E axis encoder, if present. - * - */ - void I2CPositionEncodersMgr::M863() { - if (parse()) return; +/** + * M869: Report position encoder module error. + * + * A Module I2C address. [30, 200]. + * I Module index. [0, I2CPE_ENCODER_CNT - 1]. + * + * If A not specified: + * X Act on X axis encoder, if present. + * Y Act on Y axis encoder, if present. + * Z Act on Z axis encoder, if present. + * E Act on E axis encoder, if present. + */ +void I2CPositionEncodersMgr::M869() { + if (parse()) return; - const uint8_t iterations = constrain(parser.byteval('P', 1), 1, 10); - - if (I2CPE_idx == 0xFF) { - LOOP_XYZE(i) { - if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { - const uint8_t idx = idx_from_axis(AxisEnum(i)); - if ((int8_t)idx >= 0) calibrate_steps_mm(idx, iterations); - } + if (I2CPE_idx == 0xFF) { + LOOP_XYZE(i) { + if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { + const uint8_t idx = idx_from_axis(AxisEnum(i)); + if ((int8_t)idx >= 0) report_error(idx); } } - else - calibrate_steps_mm(I2CPE_idx, iterations); - } - - /** - * M864: Change position encoder module I2C address. - * - * A Module current/old I2C address. If not present, - * assumes default address (030). [30, 200]. - * S Module new I2C address. [30, 200]. - * - * If S is not specified: - * X Use I2CPE_PRESET_ADDR_X (030). - * Y Use I2CPE_PRESET_ADDR_Y (031). - * Z Use I2CPE_PRESET_ADDR_Z (032). - * E Use I2CPE_PRESET_ADDR_E (033). - */ - void I2CPositionEncodersMgr::M864() { - uint8_t newAddress; - - if (parse()) return; - - if (!I2CPE_addr) I2CPE_addr = I2CPE_PRESET_ADDR_X; - - if (parser.seen('S')) { - if (!parser.has_value()) { - SERIAL_PROTOCOLLNPGM("?S seen, but no address specified! [30-200]"); - return; - }; - - newAddress = parser.value_byte(); - if (!WITHIN(newAddress, 30, 200)) { - SERIAL_PROTOCOLLNPGM("?New address out of range. [30-200]"); - return; - } - } - else if (!I2CPE_anyaxis) { - SERIAL_PROTOCOLLNPGM("?You must specify S or [XYZE]."); - return; - } - else { - if (parser.seen('X')) newAddress = I2CPE_PRESET_ADDR_X; - else if (parser.seen('Y')) newAddress = I2CPE_PRESET_ADDR_Y; - else if (parser.seen('Z')) newAddress = I2CPE_PRESET_ADDR_Z; - else if (parser.seen('E')) newAddress = I2CPE_PRESET_ADDR_E; - else return; - } - - SERIAL_ECHOPAIR("Changing module at address ", I2CPE_addr); - SERIAL_ECHOLNPAIR(" to address ", newAddress); - - change_module_address(I2CPE_addr, newAddress); - } - - /** - * M865: Check position encoder module firmware version. - * - * A Module I2C address. [30, 200]. - * I Module index. [0, I2CPE_ENCODER_CNT - 1]. - * - * If A or I not specified: - * X Check X axis encoder, if present. - * Y Check Y axis encoder, if present. - * Z Check Z axis encoder, if present. - * E Check E axis encoder, if present. - */ - void I2CPositionEncodersMgr::M865() { - if (parse()) return; - - if (!I2CPE_addr) { - LOOP_XYZE(i) { - if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { - const uint8_t idx = idx_from_axis(AxisEnum(i)); - if ((int8_t)idx >= 0) report_module_firmware(encoders[idx].get_address()); - } - } - } - else - report_module_firmware(I2CPE_addr); - } - - /** - * M866: Report or reset position encoder module error - * count. - * - * A Module I2C address. [30, 200]. - * I Module index. [0, I2CPE_ENCODER_CNT - 1]. - * R Reset error counter. - * - * If A or I not specified: - * X Act on X axis encoder, if present. - * Y Act on Y axis encoder, if present. - * Z Act on Z axis encoder, if present. - * E Act on E axis encoder, if present. - */ - void I2CPositionEncodersMgr::M866() { - if (parse()) return; - - const bool hasR = parser.seen('R'); - - if (I2CPE_idx == 0xFF) { - LOOP_XYZE(i) { - if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { - const uint8_t idx = idx_from_axis(AxisEnum(i)); - if ((int8_t)idx >= 0) { - if (hasR) - reset_error_count(idx, AxisEnum(i)); - else - report_error_count(idx, AxisEnum(i)); - } - } - } - } - else if (hasR) - reset_error_count(I2CPE_idx, encoders[I2CPE_idx].get_axis()); - else - report_error_count(I2CPE_idx, encoders[I2CPE_idx].get_axis()); - } - - /** - * M867: Enable/disable or toggle error correction for position encoder modules. - * - * A Module I2C address. [30, 200]. - * I Module index. [0, I2CPE_ENCODER_CNT - 1]. - * S<1|0> Enable/disable error correction. 1 enables, 0 disables. If not - * supplied, toggle. - * - * If A or I not specified: - * X Act on X axis encoder, if present. - * Y Act on Y axis encoder, if present. - * Z Act on Z axis encoder, if present. - * E Act on E axis encoder, if present. - */ - void I2CPositionEncodersMgr::M867() { - if (parse()) return; - - const int8_t onoff = parser.seenval('S') ? parser.value_int() : -1; - - if (I2CPE_idx == 0xFF) { - LOOP_XYZE(i) { - if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { - const uint8_t idx = idx_from_axis(AxisEnum(i)); - if ((int8_t)idx >= 0) { - const bool ena = onoff == -1 ? !encoders[I2CPE_idx].get_ec_enabled() : !!onoff; - enable_ec(idx, ena, AxisEnum(i)); - } - } - } - } - else { - const bool ena = onoff == -1 ? !encoders[I2CPE_idx].get_ec_enabled() : !!onoff; - enable_ec(I2CPE_idx, ena, encoders[I2CPE_idx].get_axis()); - } - } - - /** - * M868: Report or set position encoder module error correction - * threshold. - * - * A Module I2C address. [30, 200]. - * I Module index. [0, I2CPE_ENCODER_CNT - 1]. - * T New error correction threshold. - * - * If A not specified: - * X Act on X axis encoder, if present. - * Y Act on Y axis encoder, if present. - * Z Act on Z axis encoder, if present. - * E Act on E axis encoder, if present. - */ - void I2CPositionEncodersMgr::M868() { - if (parse()) return; - - const float newThreshold = parser.seenval('T') ? parser.value_float() : -9999; - - if (I2CPE_idx == 0xFF) { - LOOP_XYZE(i) { - if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { - const uint8_t idx = idx_from_axis(AxisEnum(i)); - if ((int8_t)idx >= 0) { - if (newThreshold != -9999) - set_ec_threshold(idx, newThreshold, encoders[idx].get_axis()); - else - get_ec_threshold(idx, encoders[idx].get_axis()); - } - } - } - } - else if (newThreshold != -9999) - set_ec_threshold(I2CPE_idx, newThreshold, encoders[I2CPE_idx].get_axis()); - else - get_ec_threshold(I2CPE_idx, encoders[I2CPE_idx].get_axis()); - } - - /** - * M869: Report position encoder module error. - * - * A Module I2C address. [30, 200]. - * I Module index. [0, I2CPE_ENCODER_CNT - 1]. - * - * If A not specified: - * X Act on X axis encoder, if present. - * Y Act on Y axis encoder, if present. - * Z Act on Z axis encoder, if present. - * E Act on E axis encoder, if present. - */ - void I2CPositionEncodersMgr::M869() { - if (parse()) return; - - if (I2CPE_idx == 0xFF) { - LOOP_XYZE(i) { - if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { - const uint8_t idx = idx_from_axis(AxisEnum(i)); - if ((int8_t)idx >= 0) report_error(idx); - } - } - } - else - report_error(I2CPE_idx); } + else + report_error(I2CPE_idx); +} #endif // I2C_POSITION_ENCODERS diff --git a/Marlin/src/feature/I2CPositionEncoder.h b/Marlin/src/feature/I2CPositionEncoder.h index a582a87b6..ee4305fda 100644 --- a/Marlin/src/feature/I2CPositionEncoder.h +++ b/Marlin/src/feature/I2CPositionEncoder.h @@ -23,88 +23,85 @@ #ifndef I2CPOSENC_H #define I2CPOSENC_H -#include "MarlinConfig.h" +#include "../inc/MarlinConfig.h" -#if ENABLED(I2C_POSITION_ENCODERS) +#include "../module/planner.h" - #include "enum.h" - #include "macros.h" - #include "types.h" - #include +#include - //=========== Advanced / Less-Common Encoder Configuration Settings ========== +//=========== Advanced / Less-Common Encoder Configuration Settings ========== - #define I2CPE_EC_THRESH_PROPORTIONAL // if enabled adjusts the error correction threshold - // proportional to the current speed of the axis allows - // for very small error margin at low speeds without - // stuttering due to reading latency at high speeds +#define I2CPE_EC_THRESH_PROPORTIONAL // if enabled adjusts the error correction threshold + // proportional to the current speed of the axis allows + // for very small error margin at low speeds without + // stuttering due to reading latency at high speeds - #define I2CPE_DEBUG // enable encoder-related debug serial echos +#define I2CPE_DEBUG // enable encoder-related debug serial echos - #define I2CPE_REBOOT_TIME 5000 // time we wait for an encoder module to reboot - // after changing address. +#define I2CPE_REBOOT_TIME 5000 // time we wait for an encoder module to reboot + // after changing address. - #define I2CPE_MAG_SIG_GOOD 0 - #define I2CPE_MAG_SIG_MID 1 - #define I2CPE_MAG_SIG_BAD 2 - #define I2CPE_MAG_SIG_NF 255 +#define I2CPE_MAG_SIG_GOOD 0 +#define I2CPE_MAG_SIG_MID 1 +#define I2CPE_MAG_SIG_BAD 2 +#define I2CPE_MAG_SIG_NF 255 - #define I2CPE_REQ_REPORT 0 - #define I2CPE_RESET_COUNT 1 - #define I2CPE_SET_ADDR 2 - #define I2CPE_SET_REPORT_MODE 3 - #define I2CPE_CLEAR_EEPROM 4 +#define I2CPE_REQ_REPORT 0 +#define I2CPE_RESET_COUNT 1 +#define I2CPE_SET_ADDR 2 +#define I2CPE_SET_REPORT_MODE 3 +#define I2CPE_CLEAR_EEPROM 4 - #define I2CPE_LED_PAR_MODE 10 - #define I2CPE_LED_PAR_BRT 11 - #define I2CPE_LED_PAR_RATE 14 +#define I2CPE_LED_PAR_MODE 10 +#define I2CPE_LED_PAR_BRT 11 +#define I2CPE_LED_PAR_RATE 14 - #define I2CPE_REPORT_DISTANCE 0 - #define I2CPE_REPORT_STRENGTH 1 - #define I2CPE_REPORT_VERSION 2 +#define I2CPE_REPORT_DISTANCE 0 +#define I2CPE_REPORT_STRENGTH 1 +#define I2CPE_REPORT_VERSION 2 - // Default I2C addresses - #define I2CPE_PRESET_ADDR_X 30 - #define I2CPE_PRESET_ADDR_Y 31 - #define I2CPE_PRESET_ADDR_Z 32 - #define I2CPE_PRESET_ADDR_E 33 +// Default I2C addresses +#define I2CPE_PRESET_ADDR_X 30 +#define I2CPE_PRESET_ADDR_Y 31 +#define I2CPE_PRESET_ADDR_Z 32 +#define I2CPE_PRESET_ADDR_E 33 - #define I2CPE_DEF_AXIS X_AXIS - #define I2CPE_DEF_ADDR I2CPE_PRESET_ADDR_X +#define I2CPE_DEF_AXIS X_AXIS +#define I2CPE_DEF_ADDR I2CPE_PRESET_ADDR_X - // Error event counter; tracks how many times there is an error exceeding a certain threshold - #define I2CPE_ERR_CNT_THRESH 3.00 - #define I2CPE_ERR_CNT_DEBOUNCE_MS 2000 +// Error event counter; tracks how many times there is an error exceeding a certain threshold +#define I2CPE_ERR_CNT_THRESH 3.00 +#define I2CPE_ERR_CNT_DEBOUNCE_MS 2000 - #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE) - #define I2CPE_ERR_ARRAY_SIZE 32 - #endif +#if ENABLED(I2CPE_ERR_ROLLING_AVERAGE) + #define I2CPE_ERR_ARRAY_SIZE 32 +#endif - // Error Correction Methods - #define I2CPE_ECM_NONE 0 - #define I2CPE_ECM_MICROSTEP 1 - #define I2CPE_ECM_PLANNER 2 - #define I2CPE_ECM_STALLDETECT 3 +// Error Correction Methods +#define I2CPE_ECM_NONE 0 +#define I2CPE_ECM_MICROSTEP 1 +#define I2CPE_ECM_PLANNER 2 +#define I2CPE_ECM_STALLDETECT 3 - // Encoder types - #define I2CPE_ENC_TYPE_ROTARY 0 - #define I2CPE_ENC_TYPE_LINEAR 1 +// Encoder types +#define I2CPE_ENC_TYPE_ROTARY 0 +#define I2CPE_ENC_TYPE_LINEAR 1 - // Parser - #define I2CPE_PARSE_ERR 1 - #define I2CPE_PARSE_OK 0 +// Parser +#define I2CPE_PARSE_ERR 1 +#define I2CPE_PARSE_OK 0 - #define LOOP_PE(VAR) LOOP_L_N(VAR, I2CPE_ENCODER_CNT) - #define CHECK_IDX() do{ if (!WITHIN(idx, 0, I2CPE_ENCODER_CNT - 1)) return; }while(0) +#define LOOP_PE(VAR) LOOP_L_N(VAR, I2CPE_ENCODER_CNT) +#define CHECK_IDX() do{ if (!WITHIN(idx, 0, I2CPE_ENCODER_CNT - 1)) return; }while(0) - extern const char axis_codes[XYZE]; +extern const char axis_codes[XYZE]; - typedef union { - volatile int32_t val = 0; - uint8_t bval[4]; - } i2cLong; +typedef union { + volatile int32_t val = 0; + uint8_t bval[4]; +} i2cLong; - class I2CPositionEncoder { +class I2CPositionEncoder { private: AxisEnum encoderAxis = I2CPE_DEF_AXIS; @@ -229,9 +226,9 @@ FORCE_INLINE void set_current_position(const float newPositionMm) { set_axis_offset(get_position_mm() - newPositionMm + axisOffset); } - }; +}; - class I2CPositionEncodersMgr { +class I2CPositionEncodersMgr { private: static bool I2CPE_anyaxis; static uint8_t I2CPE_addr, I2CPE_idx; @@ -252,7 +249,7 @@ static void report_status(const int8_t idx) { CHECK_IDX(); - SERIAL_ECHOPAIR("Encoder ",idx); + SERIAL_ECHOPAIR("Encoder ", idx); SERIAL_ECHOPGM(": "); encoders[idx].get_raw_count(); encoders[idx].passes_test(true); @@ -340,20 +337,19 @@ static void M869(); static I2CPositionEncoder encoders[I2CPE_ENCODER_CNT]; - }; +}; - extern I2CPositionEncodersMgr I2CPEM; +extern I2CPositionEncodersMgr I2CPEM; - FORCE_INLINE static void gcode_M860() { I2CPEM.M860(); } - FORCE_INLINE static void gcode_M861() { I2CPEM.M861(); } - FORCE_INLINE static void gcode_M862() { I2CPEM.M862(); } - FORCE_INLINE static void gcode_M863() { I2CPEM.M863(); } - FORCE_INLINE static void gcode_M864() { I2CPEM.M864(); } - FORCE_INLINE static void gcode_M865() { I2CPEM.M865(); } - FORCE_INLINE static void gcode_M866() { I2CPEM.M866(); } - FORCE_INLINE static void gcode_M867() { I2CPEM.M867(); } - FORCE_INLINE static void gcode_M868() { I2CPEM.M868(); } - FORCE_INLINE static void gcode_M869() { I2CPEM.M869(); } +FORCE_INLINE static void gcode_M860() { I2CPEM.M860(); } +FORCE_INLINE static void gcode_M861() { I2CPEM.M861(); } +FORCE_INLINE static void gcode_M862() { I2CPEM.M862(); } +FORCE_INLINE static void gcode_M863() { I2CPEM.M863(); } +FORCE_INLINE static void gcode_M864() { I2CPEM.M864(); } +FORCE_INLINE static void gcode_M865() { I2CPEM.M865(); } +FORCE_INLINE static void gcode_M866() { I2CPEM.M866(); } +FORCE_INLINE static void gcode_M867() { I2CPEM.M867(); } +FORCE_INLINE static void gcode_M868() { I2CPEM.M868(); } +FORCE_INLINE static void gcode_M869() { I2CPEM.M869(); } -#endif //I2C_POSITION_ENCODERS #endif //I2CPOSENC_H diff --git a/Marlin/src/feature/dac/dac_dac084s085.cpp b/Marlin/src/feature/dac/dac_dac084s085.cpp index b1dacdec6..c90bf79d7 100644 --- a/Marlin/src/feature/dac/dac_dac084s085.cpp +++ b/Marlin/src/feature/dac/dac_dac084s085.cpp @@ -3,106 +3,110 @@ * External DAC for Alligator Board * ****************************************************************/ -#include "Marlin.h" + +#include "../../inc/MarlinConfig.h" #if MB(ALLIGATOR) - #include "stepper.h" - #include "dac_dac084s085.h" - dac084s085::dac084s085() { - return ; - } +#include "dac_dac084s085.h" - void dac084s085::begin() { - uint8_t externalDac_buf[2] = {0x20,0x00};//all off +#include "../../Marlin.h" +#include "../../module/stepper.h" - // All SPI chip-select HIGH - pinMode(DAC0_SYNC, OUTPUT); - digitalWrite( DAC0_SYNC , HIGH ); - #if EXTRUDERS > 1 - pinMode(DAC1_SYNC, OUTPUT); - digitalWrite( DAC1_SYNC , HIGH ); - #endif - digitalWrite( SPI_EEPROM1_CS , HIGH ); - digitalWrite( SPI_EEPROM2_CS , HIGH ); - digitalWrite( SPI_FLASH_CS , HIGH ); - digitalWrite( SS_PIN , HIGH ); - spiBegin(); +dac084s085::dac084s085() { + return ; +} - //init onboard DAC +void dac084s085::begin() { + uint8_t externalDac_buf[2] = {0x20,0x00};//all off + + // All SPI chip-select HIGH + pinMode(DAC0_SYNC, OUTPUT); + digitalWrite( DAC0_SYNC , HIGH ); + #if EXTRUDERS > 1 + pinMode(DAC1_SYNC, OUTPUT); + digitalWrite( DAC1_SYNC , HIGH ); + #endif + digitalWrite( SPI_EEPROM1_CS , HIGH ); + digitalWrite( SPI_EEPROM2_CS , HIGH ); + digitalWrite( SPI_FLASH_CS , HIGH ); + digitalWrite( SS_PIN , HIGH ); + spiBegin(); + + //init onboard DAC + delayMicroseconds(2U); + digitalWrite( DAC0_SYNC , LOW ); + delayMicroseconds(2U); + digitalWrite( DAC0_SYNC , HIGH ); + delayMicroseconds(2U); + digitalWrite( DAC0_SYNC , LOW ); + + spiSend(SPI_CHAN_DAC,externalDac_buf , 2); + digitalWrite( DAC0_SYNC , HIGH ); + + #if EXTRUDERS > 1 + //init Piggy DAC delayMicroseconds(2U); - digitalWrite( DAC0_SYNC , LOW ); + digitalWrite( DAC1_SYNC , LOW ); delayMicroseconds(2U); - digitalWrite( DAC0_SYNC , HIGH ); + digitalWrite( DAC1_SYNC , HIGH ); delayMicroseconds(2U); - digitalWrite( DAC0_SYNC , LOW ); + digitalWrite( DAC1_SYNC , LOW ); spiSend(SPI_CHAN_DAC,externalDac_buf , 2); - digitalWrite( DAC0_SYNC , HIGH ); + digitalWrite( DAC1_SYNC , HIGH ); + #endif - #if EXTRUDERS > 1 - //init Piggy DAC - delayMicroseconds(2U); - digitalWrite( DAC1_SYNC , LOW ); - delayMicroseconds(2U); - digitalWrite( DAC1_SYNC , HIGH ); - delayMicroseconds(2U); - digitalWrite( DAC1_SYNC , LOW ); - - spiSend(SPI_CHAN_DAC,externalDac_buf , 2); - digitalWrite( DAC1_SYNC , HIGH ); - #endif + return; +} +void dac084s085::setValue(uint8_t channel, uint8_t value) { + if(channel >= 7) // max channel (X,Y,Z,E0,E1,E2,E3) return; - } + if(value > 255) value = 255; - void dac084s085::setValue(uint8_t channel, uint8_t value) { - if(channel >= 7) // max channel (X,Y,Z,E0,E1,E2,E3) - return; - if(value > 255) value = 255; + uint8_t externalDac_buf[2] = {0x10,0x00}; - uint8_t externalDac_buf[2] = {0x10,0x00}; + if(channel > 3) + externalDac_buf[0] |= (7 - channel << 6); + else + externalDac_buf[0] |= (3 - channel << 6); - if(channel > 3) - externalDac_buf[0] |= (7 - channel << 6); - else - externalDac_buf[0] |= (3 - channel << 6); + externalDac_buf[0] |= (value >> 4); + externalDac_buf[1] |= (value << 4); + + // All SPI chip-select HIGH + digitalWrite( DAC0_SYNC , HIGH ); + #if EXTRUDERS > 1 + digitalWrite( DAC1_SYNC , HIGH ); + #endif + digitalWrite( SPI_EEPROM1_CS , HIGH ); + digitalWrite( SPI_EEPROM2_CS , HIGH ); + digitalWrite( SPI_FLASH_CS , HIGH ); + digitalWrite( SS_PIN , HIGH ); - externalDac_buf[0] |= (value >> 4); - externalDac_buf[1] |= (value << 4); - - // All SPI chip-select HIGH - digitalWrite( DAC0_SYNC , HIGH ); - #if EXTRUDERS > 1 - digitalWrite( DAC1_SYNC , HIGH ); - #endif - digitalWrite( SPI_EEPROM1_CS , HIGH ); - digitalWrite( SPI_EEPROM2_CS , HIGH ); - digitalWrite( SPI_FLASH_CS , HIGH ); - digitalWrite( SS_PIN , HIGH ); - - if(channel > 3) { // DAC Piggy E1,E2,E3 - - digitalWrite(DAC1_SYNC , LOW); - delayMicroseconds(2U); - digitalWrite(DAC1_SYNC , HIGH); - delayMicroseconds(2U); - digitalWrite(DAC1_SYNC , LOW); - } - - else { // DAC onboard X,Y,Z,E0 - - digitalWrite(DAC0_SYNC , LOW); - delayMicroseconds(2U); - digitalWrite(DAC0_SYNC , HIGH); - delayMicroseconds(2U); - digitalWrite(DAC0_SYNC , LOW); - } + if(channel > 3) { // DAC Piggy E1,E2,E3 + digitalWrite(DAC1_SYNC , LOW); delayMicroseconds(2U); - spiSend(SPI_CHAN_DAC,externalDac_buf , 2); - - return; + digitalWrite(DAC1_SYNC , HIGH); + delayMicroseconds(2U); + digitalWrite(DAC1_SYNC , LOW); } -#endif + else { // DAC onboard X,Y,Z,E0 + + digitalWrite(DAC0_SYNC , LOW); + delayMicroseconds(2U); + digitalWrite(DAC0_SYNC , HIGH); + delayMicroseconds(2U); + digitalWrite(DAC0_SYNC , LOW); + } + + delayMicroseconds(2U); + spiSend(SPI_CHAN_DAC,externalDac_buf , 2); + + return; +} + +#endif // MB(ALLIGATOR) diff --git a/Marlin/src/feature/dac/dac_dac084s085.h b/Marlin/src/feature/dac/dac_dac084s085.h index 797103ce1..f7d823a3b 100644 --- a/Marlin/src/feature/dac/dac_dac084s085.h +++ b/Marlin/src/feature/dac/dac_dac084s085.h @@ -1,5 +1,5 @@ -#ifndef dac084s085_h -#define dac084s085_h +#ifndef DAC084S085_H +#define DAC084S085_H class dac084s085 { public: @@ -8,4 +8,4 @@ class dac084s085 { static void setValue(uint8_t channel, uint8_t value); }; -#endif //dac084s085_h +#endif // DAC084S085_H diff --git a/Marlin/src/feature/dac/dac_mcp4728.cpp b/Marlin/src/feature/dac/dac_mcp4728.cpp index a06346c6e..b10a61e9d 100644 --- a/Marlin/src/feature/dac/dac_mcp4728.cpp +++ b/Marlin/src/feature/dac/dac_mcp4728.cpp @@ -30,11 +30,12 @@ * http://arduino.cc/forum/index.php/topic,51842.0.html */ -#include "dac_mcp4728.h" -#include "enum.h" +#include "../../inc/MarlinConfig.h" #if ENABLED(DAC_STEPPER_CURRENT) +#include "dac_mcp4728.h" + uint16_t mcp4728_values[XYZE]; /** diff --git a/Marlin/src/feature/dac/dac_mcp4728.h b/Marlin/src/feature/dac/dac_mcp4728.h index f8337316d..0e999afb7 100644 --- a/Marlin/src/feature/dac/dac_mcp4728.h +++ b/Marlin/src/feature/dac/dac_mcp4728.h @@ -27,10 +27,7 @@ #ifndef DAC_MCP4728_H #define DAC_MCP4728_H -#include "MarlinConfig.h" - -#if ENABLED(DAC_STEPPER_CURRENT) -#include "Wire.h" +#include #define defaultVDD DAC_STEPPER_MAX //was 5000 but differs with internal Vref #define BASE_ADDR 0x60 @@ -50,7 +47,6 @@ // DAC_OR_ADDRESS defined in pins_BOARD.h file #define DAC_DEV_ADDRESS (BASE_ADDR | DAC_OR_ADDRESS) - void mcp4728_init(); uint8_t mcp4728_analogWrite(uint8_t channel, uint16_t value); uint8_t mcp4728_eepromWrite(); @@ -62,5 +58,4 @@ uint8_t mcp4728_simpleCommand(byte simpleCommand); uint8_t mcp4728_getDrvPct(uint8_t channel); void mcp4728_setDrvPct(uint8_t pct[XYZE]); -#endif #endif // DAC_MCP4728_H diff --git a/Marlin/src/feature/dac/stepper_dac.cpp b/Marlin/src/feature/dac/stepper_dac.cpp index 6ea8b83bc..f43805350 100644 --- a/Marlin/src/feature/dac/stepper_dac.cpp +++ b/Marlin/src/feature/dac/stepper_dac.cpp @@ -41,85 +41,85 @@ along with Marlin. If not, see . */ -#include "Marlin.h" +#include "../../inc/MarlinConfig.h" #if ENABLED(DAC_STEPPER_CURRENT) - #include "stepper_dac.h" +#include "stepper_dac.h" - bool dac_present = false; - const uint8_t dac_order[NUM_AXIS] = DAC_STEPPER_ORDER; - uint8_t dac_channel_pct[XYZE] = DAC_MOTOR_CURRENT_DEFAULT; +bool dac_present = false; +const uint8_t dac_order[NUM_AXIS] = DAC_STEPPER_ORDER; +uint8_t dac_channel_pct[XYZE] = DAC_MOTOR_CURRENT_DEFAULT; - int dac_init() { - #if PIN_EXISTS(DAC_DISABLE) - OUT_WRITE(DAC_DISABLE_PIN, LOW); // set pin low to enable DAC - #endif +int dac_init() { + #if PIN_EXISTS(DAC_DISABLE) + OUT_WRITE(DAC_DISABLE_PIN, LOW); // set pin low to enable DAC + #endif - mcp4728_init(); + mcp4728_init(); - if (mcp4728_simpleCommand(RESET)) return -1; + if (mcp4728_simpleCommand(RESET)) return -1; - dac_present = true; + dac_present = true; - mcp4728_setVref_all(DAC_STEPPER_VREF); - mcp4728_setGain_all(DAC_STEPPER_GAIN); + mcp4728_setVref_all(DAC_STEPPER_VREF); + mcp4728_setGain_all(DAC_STEPPER_GAIN); - if (mcp4728_getDrvPct(0) < 1 || mcp4728_getDrvPct(1) < 1 || mcp4728_getDrvPct(2) < 1 || mcp4728_getDrvPct(3) < 1 ) { - mcp4728_setDrvPct(dac_channel_pct); - mcp4728_eepromWrite(); - } - - return 0; - } - - void dac_current_percent(uint8_t channel, float val) { - if (!dac_present) return; - - NOMORE(val, 100); - - mcp4728_analogWrite(dac_order[channel], val * 0.01 * (DAC_STEPPER_MAX)); - mcp4728_simpleCommand(UPDATE); - } - - void dac_current_raw(uint8_t channel, uint16_t val) { - if (!dac_present) return; - - NOMORE(val, DAC_STEPPER_MAX); - - mcp4728_analogWrite(dac_order[channel], val); - mcp4728_simpleCommand(UPDATE); - } - - static float dac_perc(int8_t n) { return 100.0 * mcp4728_getValue(dac_order[n]) * (1.0 / (DAC_STEPPER_MAX)); } - static float dac_amps(int8_t n) { return mcp4728_getDrvPct(dac_order[n]) * (DAC_STEPPER_MAX) * 0.125 * (1.0 / (DAC_STEPPER_SENSE)); } - - uint8_t dac_current_get_percent(AxisEnum axis) { return mcp4728_getDrvPct(dac_order[axis]); } - void dac_current_set_percents(const uint8_t pct[XYZE]) { - LOOP_XYZE(i) dac_channel_pct[i] = pct[dac_order[i]]; + if (mcp4728_getDrvPct(0) < 1 || mcp4728_getDrvPct(1) < 1 || mcp4728_getDrvPct(2) < 1 || mcp4728_getDrvPct(3) < 1 ) { mcp4728_setDrvPct(dac_channel_pct); - } - - void dac_print_values() { - if (!dac_present) return; - - SERIAL_ECHO_START(); - SERIAL_ECHOLNPGM("Stepper current values in % (Amps):"); - SERIAL_ECHO_START(); - SERIAL_ECHOPAIR(" X:", dac_perc(X_AXIS)); - SERIAL_ECHOPAIR(" (", dac_amps(X_AXIS)); - SERIAL_ECHOPAIR(") Y:", dac_perc(Y_AXIS)); - SERIAL_ECHOPAIR(" (", dac_amps(Y_AXIS)); - SERIAL_ECHOPAIR(") Z:", dac_perc(Z_AXIS)); - SERIAL_ECHOPAIR(" (", dac_amps(Z_AXIS)); - SERIAL_ECHOPAIR(") E:", dac_perc(E_AXIS)); - SERIAL_ECHOPAIR(" (", dac_amps(E_AXIS)); - SERIAL_ECHOLN(")"); - } - - void dac_commit_eeprom() { - if (!dac_present) return; mcp4728_eepromWrite(); } + return 0; +} + +void dac_current_percent(uint8_t channel, float val) { + if (!dac_present) return; + + NOMORE(val, 100); + + mcp4728_analogWrite(dac_order[channel], val * 0.01 * (DAC_STEPPER_MAX)); + mcp4728_simpleCommand(UPDATE); +} + +void dac_current_raw(uint8_t channel, uint16_t val) { + if (!dac_present) return; + + NOMORE(val, DAC_STEPPER_MAX); + + mcp4728_analogWrite(dac_order[channel], val); + mcp4728_simpleCommand(UPDATE); +} + +static float dac_perc(int8_t n) { return 100.0 * mcp4728_getValue(dac_order[n]) * (1.0 / (DAC_STEPPER_MAX)); } +static float dac_amps(int8_t n) { return mcp4728_getDrvPct(dac_order[n]) * (DAC_STEPPER_MAX) * 0.125 * (1.0 / (DAC_STEPPER_SENSE)); } + +uint8_t dac_current_get_percent(AxisEnum axis) { return mcp4728_getDrvPct(dac_order[axis]); } +void dac_current_set_percents(const uint8_t pct[XYZE]) { + LOOP_XYZE(i) dac_channel_pct[i] = pct[dac_order[i]]; + mcp4728_setDrvPct(dac_channel_pct); +} + +void dac_print_values() { + if (!dac_present) return; + + SERIAL_ECHO_START(); + SERIAL_ECHOLNPGM("Stepper current values in % (Amps):"); + SERIAL_ECHO_START(); + SERIAL_ECHOPAIR(" X:", dac_perc(X_AXIS)); + SERIAL_ECHOPAIR(" (", dac_amps(X_AXIS)); + SERIAL_ECHOPAIR(") Y:", dac_perc(Y_AXIS)); + SERIAL_ECHOPAIR(" (", dac_amps(Y_AXIS)); + SERIAL_ECHOPAIR(") Z:", dac_perc(Z_AXIS)); + SERIAL_ECHOPAIR(" (", dac_amps(Z_AXIS)); + SERIAL_ECHOPAIR(") E:", dac_perc(E_AXIS)); + SERIAL_ECHOPAIR(" (", dac_amps(E_AXIS)); + SERIAL_ECHOLN(")"); +} + +void dac_commit_eeprom() { + if (!dac_present) return; + mcp4728_eepromWrite(); +} + #endif // DAC_STEPPER_CURRENT diff --git a/Marlin/src/feature/digipot_mcp4018.cpp b/Marlin/src/feature/digipot_mcp4018.cpp index 06622d057..d57d2233e 100644 --- a/Marlin/src/feature/digipot_mcp4018.cpp +++ b/Marlin/src/feature/digipot_mcp4018.cpp @@ -20,11 +20,11 @@ * */ -#include "MarlinConfig.h" +#include "../inc/MarlinConfig.h" #if ENABLED(DIGIPOT_I2C) && ENABLED(DIGIPOT_MCP4018) -#include "enum.h" +#include "../core/enum.h" #include "Stream.h" #include "utility/twi.h" #include //https://github.com/stawel/SlowSoftI2CMaster diff --git a/Marlin/src/feature/digipot_mcp4451.cpp b/Marlin/src/feature/digipot_mcp4451.cpp index d79915cc9..5e94898ea 100644 --- a/Marlin/src/feature/digipot_mcp4451.cpp +++ b/Marlin/src/feature/digipot_mcp4451.cpp @@ -20,7 +20,7 @@ * */ -#include "MarlinConfig.h" +#include "../inc/MarlinConfig.h" #if ENABLED(DIGIPOT_I2C) && DISABLED(DIGIPOT_MCP4018) diff --git a/Marlin/src/feature/leds/Max7219_Debug_LEDs.cpp b/Marlin/src/feature/leds/Max7219_Debug_LEDs.cpp index d6110053f..379c11416 100644 --- a/Marlin/src/feature/leds/Max7219_Debug_LEDs.cpp +++ b/Marlin/src/feature/leds/Max7219_Debug_LEDs.cpp @@ -49,15 +49,16 @@ * void Max7219_idle_tasks(); */ -#include "MarlinConfig.h" +#include "../../inc/MarlinConfig.h" #if ENABLED(MAX7219_DEBUG) - #include "Marlin.h" - #include "planner.h" - #include "stepper.h" #include "Max7219_Debug_LEDs.h" + #include "../../module/planner.h" + #include "../../module/stepper.h" + #include "../../Marlin.h" + static uint8_t LEDs[8] = { 0 }; void Max7219_PutByte(uint8_t data) { diff --git a/Marlin/src/feature/leds/blinkm.cpp b/Marlin/src/feature/leds/blinkm.cpp index 1caf0a071..b3a2f61d0 100644 --- a/Marlin/src/feature/leds/blinkm.cpp +++ b/Marlin/src/feature/leds/blinkm.cpp @@ -25,7 +25,7 @@ * Created by Tim Koster, August 21 2013. */ -#include "Marlin.h" +#include "../../Marlin.h" #if ENABLED(BLINKM) diff --git a/Marlin/src/feature/leds/pca9632.cpp b/Marlin/src/feature/leds/pca9632.cpp index 37f7bd7df..8699cebf9 100644 --- a/Marlin/src/feature/leds/pca9632.cpp +++ b/Marlin/src/feature/leds/pca9632.cpp @@ -20,12 +20,12 @@ * */ -/* +/** * Driver for the Philips PCA9632 LED driver. * Written by Robert Mendon Feb 2017. */ -#include "MarlinConfig.h" +#include "../../inc/MarlinConfig.h" #if ENABLED(PCA9632) diff --git a/Marlin/src/feature/mbl/mesh_bed_leveling.cpp b/Marlin/src/feature/mbl/mesh_bed_leveling.cpp index 3da19d97e..3e2942789 100644 --- a/Marlin/src/feature/mbl/mesh_bed_leveling.cpp +++ b/Marlin/src/feature/mbl/mesh_bed_leveling.cpp @@ -20,10 +20,12 @@ * */ -#include "mesh_bed_leveling.h" +#include "../../inc/MarlinConfig.h" #if ENABLED(MESH_BED_LEVELING) + #include "mesh_bed_leveling.h" + mesh_bed_leveling mbl; uint8_t mesh_bed_leveling::status; diff --git a/Marlin/src/feature/mbl/mesh_bed_leveling.h b/Marlin/src/feature/mbl/mesh_bed_leveling.h index f7b701bf2..f22d56531 100644 --- a/Marlin/src/feature/mbl/mesh_bed_leveling.h +++ b/Marlin/src/feature/mbl/mesh_bed_leveling.h @@ -20,103 +20,104 @@ * */ -#include "Marlin.h" +#ifndef _MESH_BED_LEVELING_H_ +#define _MESH_BED_LEVELING_H_ -#if ENABLED(MESH_BED_LEVELING) +#include "../../Marlin.h" - enum MeshLevelingState { - MeshReport, - MeshStart, - MeshNext, - MeshSet, - MeshSetZOffset, - MeshReset - }; +enum MeshLevelingState { + MeshReport, + MeshStart, + MeshNext, + MeshSet, + MeshSetZOffset, + MeshReset +}; - enum MBLStatus { - MBL_STATUS_NONE = 0, - MBL_STATUS_HAS_MESH_BIT = 0, - MBL_STATUS_ACTIVE_BIT = 1 - }; +enum MBLStatus { + MBL_STATUS_NONE = 0, + MBL_STATUS_HAS_MESH_BIT = 0, + MBL_STATUS_ACTIVE_BIT = 1 +}; - #define MESH_X_DIST ((MESH_MAX_X - (MESH_MIN_X)) / (GRID_MAX_POINTS_X - 1)) - #define MESH_Y_DIST ((MESH_MAX_Y - (MESH_MIN_Y)) / (GRID_MAX_POINTS_Y - 1)) +#define MESH_X_DIST ((MESH_MAX_X - (MESH_MIN_X)) / (GRID_MAX_POINTS_X - 1)) +#define MESH_Y_DIST ((MESH_MAX_Y - (MESH_MIN_Y)) / (GRID_MAX_POINTS_Y - 1)) - class mesh_bed_leveling { - public: - static uint8_t status; // Has Mesh and Is Active bits - static float z_offset, - z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y], - index_to_xpos[GRID_MAX_POINTS_X], - index_to_ypos[GRID_MAX_POINTS_Y]; +class mesh_bed_leveling { +public: + static uint8_t status; // Has Mesh and Is Active bits + static float z_offset, + z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y], + index_to_xpos[GRID_MAX_POINTS_X], + index_to_ypos[GRID_MAX_POINTS_Y]; - mesh_bed_leveling(); + mesh_bed_leveling(); - static void reset(); + static void reset(); - static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; } + static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; } - static bool active() { return TEST(status, MBL_STATUS_ACTIVE_BIT); } - static void set_active(const bool onOff) { onOff ? SBI(status, MBL_STATUS_ACTIVE_BIT) : CBI(status, MBL_STATUS_ACTIVE_BIT); } - static bool has_mesh() { return TEST(status, MBL_STATUS_HAS_MESH_BIT); } - static void set_has_mesh(const bool onOff) { onOff ? SBI(status, MBL_STATUS_HAS_MESH_BIT) : CBI(status, MBL_STATUS_HAS_MESH_BIT); } + static bool active() { return TEST(status, MBL_STATUS_ACTIVE_BIT); } + static void set_active(const bool onOff) { onOff ? SBI(status, MBL_STATUS_ACTIVE_BIT) : CBI(status, MBL_STATUS_ACTIVE_BIT); } + static bool has_mesh() { return TEST(status, MBL_STATUS_HAS_MESH_BIT); } + static void set_has_mesh(const bool onOff) { onOff ? SBI(status, MBL_STATUS_HAS_MESH_BIT) : CBI(status, MBL_STATUS_HAS_MESH_BIT); } - static inline void zigzag(const int8_t index, int8_t &px, int8_t &py) { - px = index % (GRID_MAX_POINTS_X); - py = index / (GRID_MAX_POINTS_X); - if (py & 1) px = (GRID_MAX_POINTS_X - 1) - px; // Zig zag - } + static inline void zigzag(const int8_t index, int8_t &px, int8_t &py) { + px = index % (GRID_MAX_POINTS_X); + py = index / (GRID_MAX_POINTS_X); + if (py & 1) px = (GRID_MAX_POINTS_X - 1) - px; // Zig zag + } - static void set_zigzag_z(const int8_t index, const float &z) { - int8_t px, py; - zigzag(index, px, py); - set_z(px, py, z); - } + static void set_zigzag_z(const int8_t index, const float &z) { + int8_t px, py; + zigzag(index, px, py); + set_z(px, py, z); + } - static int8_t cell_index_x(const float &x) { - int8_t cx = (x - (MESH_MIN_X)) * (1.0 / (MESH_X_DIST)); - return constrain(cx, 0, (GRID_MAX_POINTS_X) - 2); - } + static int8_t cell_index_x(const float &x) { + int8_t cx = (x - (MESH_MIN_X)) * (1.0 / (MESH_X_DIST)); + return constrain(cx, 0, (GRID_MAX_POINTS_X) - 2); + } - static int8_t cell_index_y(const float &y) { - int8_t cy = (y - (MESH_MIN_Y)) * (1.0 / (MESH_Y_DIST)); - return constrain(cy, 0, (GRID_MAX_POINTS_Y) - 2); - } + static int8_t cell_index_y(const float &y) { + int8_t cy = (y - (MESH_MIN_Y)) * (1.0 / (MESH_Y_DIST)); + return constrain(cy, 0, (GRID_MAX_POINTS_Y) - 2); + } - static int8_t probe_index_x(const float &x) { - int8_t px = (x - (MESH_MIN_X) + 0.5 * (MESH_X_DIST)) * (1.0 / (MESH_X_DIST)); - return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1; - } + static int8_t probe_index_x(const float &x) { + int8_t px = (x - (MESH_MIN_X) + 0.5 * (MESH_X_DIST)) * (1.0 / (MESH_X_DIST)); + return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1; + } - static int8_t probe_index_y(const float &y) { - int8_t py = (y - (MESH_MIN_Y) + 0.5 * (MESH_Y_DIST)) * (1.0 / (MESH_Y_DIST)); - return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1; - } + static int8_t probe_index_y(const float &y) { + int8_t py = (y - (MESH_MIN_Y) + 0.5 * (MESH_Y_DIST)) * (1.0 / (MESH_Y_DIST)); + return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1; + } - static float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) { - const float delta_z = (z2 - z1) / (a2 - a1), - delta_a = a0 - a1; - return z1 + delta_a * delta_z; - } + static float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) { + const float delta_z = (z2 - z1) / (a2 - a1), + delta_a = a0 - a1; + return z1 + delta_a * delta_z; + } - static float get_z(const float &x0, const float &y0 + static float get_z(const float &x0, const float &y0 + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) + , const float &factor + #endif + ) { + const int8_t cx = cell_index_x(x0), cy = cell_index_y(y0); + const float z1 = calc_z0(x0, index_to_xpos[cx], z_values[cx][cy], index_to_xpos[cx + 1], z_values[cx + 1][cy]), + z2 = calc_z0(x0, index_to_xpos[cx], z_values[cx][cy + 1], index_to_xpos[cx + 1], z_values[cx + 1][cy + 1]), + z0 = calc_z0(y0, index_to_ypos[cy], z1, index_to_ypos[cy + 1], z2); + + return z_offset + z0 #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) - , const float &factor + * factor #endif - ) { - const int8_t cx = cell_index_x(x0), cy = cell_index_y(y0); - const float z1 = calc_z0(x0, index_to_xpos[cx], z_values[cx][cy], index_to_xpos[cx + 1], z_values[cx + 1][cy]), - z2 = calc_z0(x0, index_to_xpos[cx], z_values[cx][cy + 1], index_to_xpos[cx + 1], z_values[cx + 1][cy + 1]), - z0 = calc_z0(y0, index_to_ypos[cy], z1, index_to_ypos[cy + 1], z2); + ; + } +}; - return z_offset + z0 - #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) - * factor - #endif - ; - } - }; +extern mesh_bed_leveling mbl; - extern mesh_bed_leveling mbl; - -#endif // MESH_BED_LEVELING +#endif // _MESH_BED_LEVELING_H_ diff --git a/Marlin/src/feature/tmc2130.cpp b/Marlin/src/feature/tmc2130.cpp new file mode 100644 index 000000000..59e44a5f2 --- /dev/null +++ b/Marlin/src/feature/tmc2130.cpp @@ -0,0 +1,157 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include "../inc/MarlinConfig.h" + +#if ENABLED(HAVE_TMC2130) + +#include "tmc2130.h" +#include "../Marlin.h" + +#include "../libs/duration_t.h" +#include "../module/stepper_indirection.h" + +#include + +#ifdef AUTOMATIC_CURRENT_CONTROL + bool auto_current_control = 0; +#endif + +void automatic_current_control(TMC2130Stepper &st, String axisID) { + // Check otpw even if we don't use automatic control. Allows for flag inspection. + const bool is_otpw = st.checkOT(); + + // Report if a warning was triggered + static bool previous_otpw = false; + if (is_otpw && !previous_otpw) { + char timestamp[10]; + duration_t elapsed = print_job_timer.duration(); + const bool has_days = (elapsed.value > 60*60*24L); + (void)elapsed.toDigital(timestamp, has_days); + SERIAL_ECHO(timestamp); + SERIAL_ECHOPGM(": "); + SERIAL_ECHO(axisID); + SERIAL_ECHOLNPGM(" driver overtemperature warning!"); + } + previous_otpw = is_otpw; + + #if ENABLED(AUTOMATIC_CURRENT_CONTROL) && CURRENT_STEP > 0 + // Return if user has not enabled current control start with M906 S1. + if (!auto_current_control) return; + + /** + * Decrease current if is_otpw is true. + * Bail out if driver is disabled. + * Increase current if OTPW has not been triggered yet. + */ + uint16_t current = st.getCurrent(); + if (is_otpw) { + st.setCurrent(current - CURRENT_STEP, R_SENSE, HOLD_MULTIPLIER); + #if ENABLED(REPORT_CURRENT_CHANGE) + SERIAL_ECHO(axisID); + SERIAL_ECHOPAIR(" current decreased to ", st.getCurrent()); + #endif + } + + else if (!st.isEnabled()) + return; + + else if (!is_otpw && !st.getOTPW()) { + current += CURRENT_STEP; + if (current <= AUTO_ADJUST_MAX) { + st.setCurrent(current, R_SENSE, HOLD_MULTIPLIER); + #if ENABLED(REPORT_CURRENT_CHANGE) + SERIAL_ECHO(axisID); + SERIAL_ECHOPAIR(" current increased to ", st.getCurrent()); + #endif + } + } + SERIAL_EOL(); + #endif +} + +void tmc2130_checkOverTemp(void) { + static millis_t next_cOT = 0; + if (ELAPSED(millis(), next_cOT)) { + next_cOT = millis() + 5000; + #if ENABLED(X_IS_TMC2130) + automatic_current_control(stepperX, "X"); + #endif + #if ENABLED(Y_IS_TMC2130) + automatic_current_control(stepperY, "Y"); + #endif + #if ENABLED(Z_IS_TMC2130) + automatic_current_control(stepperZ, "Z"); + #endif + #if ENABLED(X2_IS_TMC2130) + automatic_current_control(stepperX2, "X2"); + #endif + #if ENABLED(Y2_IS_TMC2130) + automatic_current_control(stepperY2, "Y2"); + #endif + #if ENABLED(Z2_IS_TMC2130) + automatic_current_control(stepperZ2, "Z2"); + #endif + #if ENABLED(E0_IS_TMC2130) + automatic_current_control(stepperE0, "E0"); + #endif + #if ENABLED(E1_IS_TMC2130) + automatic_current_control(stepperE1, "E1"); + #endif + #if ENABLED(E2_IS_TMC2130) + automatic_current_control(stepperE2, "E2"); + #endif + #if ENABLED(E3_IS_TMC2130) + automatic_current_control(stepperE3, "E3"); + #endif + #if ENABLED(E4_IS_TMC2130) + automatic_current_control(stepperE4, "E4"); + #endif + #if ENABLED(E4_IS_TMC2130) + automatic_current_control(stepperE4); + #endif + } +} + +/** + * TMC2130 specific sensorless homing using stallGuard2. + * stallGuard2 only works when in spreadCycle mode. + * spreadCycle and stealthChop are mutually exclusive. + */ +#if ENABLED(SENSORLESS_HOMING) + void tmc2130_sensorless_homing(TMC2130Stepper &st, bool enable/*=true*/) { + #if ENABLED(STEALTHCHOP) + if (enable) { + st.coolstep_min_speed(1024UL * 1024UL - 1UL); + st.stealthChop(0); + } + else { + st.coolstep_min_speed(0); + st.stealthChop(1); + } + #endif + + st.diag1_stall(enable ? 1 : 0); + } +#endif // SENSORLESS_HOMING + +#endif // HAVE_TMC2130 diff --git a/Marlin/src/feature/tmc2130.h b/Marlin/src/feature/tmc2130.h new file mode 100644 index 000000000..bcaa78f9e --- /dev/null +++ b/Marlin/src/feature/tmc2130.h @@ -0,0 +1,30 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#ifndef _TMC2130_H_ +#define _TMC2130_H_ + +extern bool auto_current_control; + +void tmc2130_checkOverTemp(void); + +#endif // _TMC2130_H_ diff --git a/Marlin/src/feature/twibus.cpp b/Marlin/src/feature/twibus.cpp index bd3831af9..ef1fa742a 100644 --- a/Marlin/src/feature/twibus.cpp +++ b/Marlin/src/feature/twibus.cpp @@ -20,11 +20,12 @@ * */ -#include "Marlin.h" +#include "../inc/MarlinConfig.h" #if ENABLED(EXPERIMENTAL_I2CBUS) #include "twibus.h" + #include TWIBus::TWIBus() { diff --git a/Marlin/src/feature/twibus.h b/Marlin/src/feature/twibus.h index 03763972a..7a17e2d53 100644 --- a/Marlin/src/feature/twibus.h +++ b/Marlin/src/feature/twibus.h @@ -23,7 +23,7 @@ #ifndef TWIBUS_H #define TWIBUS_H -#include "macros.h" +#include "../core/macros.h" #include diff --git a/Marlin/src/feature/ubl/G26_Mesh_Validation_Tool.cpp b/Marlin/src/feature/ubl/G26_Mesh_Validation_Tool.cpp index 9b1d4b5ad..23b1b96fb 100644 --- a/Marlin/src/feature/ubl/G26_Mesh_Validation_Tool.cpp +++ b/Marlin/src/feature/ubl/G26_Mesh_Validation_Tool.cpp @@ -24,17 +24,18 @@ * Marlin Firmware -- G26 - Mesh Validation Tool */ -#include "MarlinConfig.h" +#include "../../inc/MarlinConfig.h" #if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_G26_MESH_VALIDATION) #include "ubl.h" - #include "Marlin.h" - #include "planner.h" - #include "stepper.h" - #include "temperature.h" - #include "ultralcd.h" - #include "gcode.h" + + #include "../../Marlin.h" + #include "../../module/planner.h" + #include "../../module/stepper.h" + #include "../../module/temperature.h" + #include "../../lcd/ultralcd.h" + #include "../../gcode/parser.h" #define EXTRUSION_MULTIPLIER 1.0 #define RETRACTION_MULTIPLIER 1.0 @@ -140,8 +141,8 @@ inline void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]); } inline void set_current_to_destination() { COPY(current_position, destination); } #else - extern void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]); } - extern void set_current_to_destination() { COPY(current_position, destination); } + extern void sync_plan_position_e(); + extern void set_current_to_destination(); #endif #if ENABLED(NEWPANEL) void lcd_setstatusPGM(const char* const message, const int8_t level); diff --git a/Marlin/src/feature/ubl/ubl.cpp b/Marlin/src/feature/ubl/ubl.cpp index 9805aff3f..7c85db521 100644 --- a/Marlin/src/feature/ubl/ubl.cpp +++ b/Marlin/src/feature/ubl/ubl.cpp @@ -20,16 +20,18 @@ * */ -#include "Marlin.h" -#include "math.h" +#include "../../inc/MarlinConfig.h" #if ENABLED(AUTO_BED_LEVELING_UBL) #include "ubl.h" - #include "hex_print_routines.h" - #include "temperature.h" + unified_bed_leveling ubl; - extern Planner planner; + #include "../../module/configuration_store.h" + #include "../../core/serial.h" + #include "../../module/planner.h" + + #include "math.h" /** * These support functions allow the use of large bit arrays of flags that take very @@ -37,9 +39,9 @@ * to unsigned long will allow us to go to 32x32 if higher resolution Mesh's are needed * in the future. */ - void bit_clear(uint16_t bits[16], uint8_t x, uint8_t y) { CBI(bits[y], x); } - void bit_set(uint16_t bits[16], uint8_t x, uint8_t y) { SBI(bits[y], x); } - bool is_bit_set(uint16_t bits[16], uint8_t x, uint8_t y) { return TEST(bits[y], x); } + void bit_clear(uint16_t bits[16], const uint8_t x, const uint8_t y) { CBI(bits[y], x); } + void bit_set(uint16_t bits[16], const uint8_t x, const uint8_t y) { SBI(bits[y], x); } + bool is_bit_set(uint16_t bits[16], const uint8_t x, const uint8_t y) { return TEST(bits[y], x); } uint8_t ubl_cnt = 0; diff --git a/Marlin/src/feature/ubl/ubl.h b/Marlin/src/feature/ubl/ubl.h index b1d3bed78..b6a72871b 100644 --- a/Marlin/src/feature/ubl/ubl.h +++ b/Marlin/src/feature/ubl/ubl.h @@ -23,387 +23,375 @@ #ifndef UNIFIED_BED_LEVELING_H #define UNIFIED_BED_LEVELING_H -#include "MarlinConfig.h" +#include "../../Marlin.h" +#include "../../core/serial.h" +#include "../../module/planner.h" -#if ENABLED(AUTO_BED_LEVELING_UBL) - #include "Marlin.h" - #include "planner.h" - #include "math.h" - #include "vector_3.h" - #include "configuration_store.h" +#define UBL_VERSION "1.01" +#define UBL_OK false +#define UBL_ERR true - #define UBL_VERSION "1.01" - #define UBL_OK false - #define UBL_ERR true +#define USE_NOZZLE_AS_REFERENCE 0 +#define USE_PROBE_AS_REFERENCE 1 - #define USE_NOZZLE_AS_REFERENCE 0 - #define USE_PROBE_AS_REFERENCE 1 +typedef struct { + int8_t x_index, y_index; + float distance; // When populated, the distance from the search location +} mesh_index_pair; - typedef struct { - int8_t x_index, y_index; - float distance; // When populated, the distance from the search location - } mesh_index_pair; +// ubl.cpp - // ubl.cpp +void bit_clear(uint16_t bits[16], const uint8_t x, const uint8_t y); +void bit_set(uint16_t bits[16], const uint8_t x, const uint8_t y); +bool is_bit_set(uint16_t bits[16], const uint8_t x, const uint8_t y); - void bit_clear(uint16_t bits[16], uint8_t x, uint8_t y); - void bit_set(uint16_t bits[16], uint8_t x, uint8_t y); - bool is_bit_set(uint16_t bits[16], uint8_t x, uint8_t y); +// ubl_motion.cpp - // ubl_motion.cpp +void debug_current_and_destination(const char * const title); - void debug_current_and_destination(const char * const title); +// ubl_G29.cpp - // ubl_G29.cpp +enum MeshPointType { INVALID, REAL, SET_IN_BITMAP }; - enum MeshPointType { INVALID, REAL, SET_IN_BITMAP }; +// External references - // External references +char *ftostr43sign(const float&, char); +bool ubl_lcd_clicked(); +void home_all_axes(); - char *ftostr43sign(const float&, char); - bool ubl_lcd_clicked(); - void home_all_axes(); +extern uint8_t ubl_cnt; - extern uint8_t ubl_cnt; +/////////////////////////////////////////////////////////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////////////////////////////////////// +#if ENABLED(ULTRA_LCD) + extern char lcd_status_message[]; + void lcd_quick_feedback(); +#endif - #if ENABLED(ULTRA_LCD) - extern char lcd_status_message[]; - void lcd_quick_feedback(); - #endif +#define MESH_X_DIST (float(UBL_MESH_MAX_X - (UBL_MESH_MIN_X)) / float(GRID_MAX_POINTS_X - 1)) +#define MESH_Y_DIST (float(UBL_MESH_MAX_Y - (UBL_MESH_MIN_Y)) / float(GRID_MAX_POINTS_Y - 1)) - #define MESH_X_DIST (float(UBL_MESH_MAX_X - (UBL_MESH_MIN_X)) / float(GRID_MAX_POINTS_X - 1)) - #define MESH_Y_DIST (float(UBL_MESH_MAX_Y - (UBL_MESH_MIN_Y)) / float(GRID_MAX_POINTS_Y - 1)) +typedef struct { + bool active = false; + float z_offset = 0.0; + int8_t storage_slot = -1; +} ubl_state; - typedef struct { - bool active = false; - float z_offset = 0.0; - int8_t storage_slot = -1; - } ubl_state; +class unified_bed_leveling { + private: - class unified_bed_leveling { - private: + static float last_specified_z; - static float last_specified_z; + static int g29_verbose_level, + g29_phase_value, + g29_repetition_cnt, + g29_storage_slot, + g29_map_type, + g29_grid_size; + static bool g29_c_flag, g29_x_flag, g29_y_flag; + static float g29_x_pos, g29_y_pos, + g29_card_thickness, + g29_constant; - static int g29_verbose_level, - g29_phase_value, - g29_repetition_cnt, - g29_storage_slot, - g29_map_type, - g29_grid_size; - static bool g29_c_flag, g29_x_flag, g29_y_flag; - static float g29_x_pos, g29_y_pos, - g29_card_thickness, - g29_constant; + #if ENABLED(UBL_G26_MESH_VALIDATION) + static float g26_extrusion_multiplier, + g26_retraction_multiplier, + g26_nozzle, + g26_filament_diameter, + g26_prime_length, + g26_x_pos, g26_y_pos, + g26_ooze_amount, + g26_layer_height; + static int16_t g26_bed_temp, + g26_hotend_temp, + g26_repeats; + static int8_t g26_prime_flag; + static bool g26_continue_with_closest, g26_keep_heaters_on; + #endif - #if ENABLED(UBL_G26_MESH_VALIDATION) - static float g26_extrusion_multiplier, - g26_retraction_multiplier, - g26_nozzle, - g26_filament_diameter, - g26_prime_length, - g26_x_pos, g26_y_pos, - g26_ooze_amount, - g26_layer_height; - static int16_t g26_bed_temp, - g26_hotend_temp, - g26_repeats; - static int8_t g26_prime_flag; - static bool g26_continue_with_closest, g26_keep_heaters_on; + static float measure_point_with_encoder(); + static float measure_business_card_thickness(float); + static bool g29_parameter_parsing(); + static void find_mean_mesh_height(); + static void shift_mesh_height(); + static void probe_entire_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map, const bool stow_probe, bool do_furthest); + static void manually_probe_remaining_mesh(const float&, const float&, const float&, const float&, const bool); + static void tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3); + static void tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map); + static void g29_what_command(); + static void g29_eeprom_dump(); + static void g29_compare_current_mesh_to_stored_mesh(); + static void fine_tune_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map); + static bool smart_fill_one(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir); + static void smart_fill_mesh(); + + #if ENABLED(UBL_G26_MESH_VALIDATION) + static bool exit_from_g26(); + static bool parse_G26_parameters(); + static void G26_line_to_destination(const float &feed_rate); + static mesh_index_pair find_closest_circle_to_print(const float&, const float&); + static bool look_for_lines_to_connect(); + static bool turn_on_heaters(); + static bool prime_nozzle(); + static void retract_filament(const float where[XYZE]); + static void recover_filament(const float where[XYZE]); + static void print_line_from_here_to_there(const float&, const float&, const float&, const float&, const float&, const float&); + static void move_to(const float&, const float&, const float&, const float&); + inline static void move_to(const float where[XYZE], const float &de) { move_to(where[X_AXIS], where[Y_AXIS], where[Z_AXIS], de); } + #endif + + public: + + static void echo_name(); + static void report_state(); + static void save_ubl_active_state_and_disable(); + static void restore_ubl_active_state_and_leave(); + static void display_map(const int); + static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, uint16_t[16], bool); + static void reset(); + static void invalidate(); + static void set_all_mesh_points_to_value(float); + static bool sanity_check(); + + static void G29() _O0; // O0 for no optimization + static void smart_fill_wlsf(const float &) _O2; // O2 gives smaller code than Os on A2560 + + #if ENABLED(UBL_G26_MESH_VALIDATION) + static void G26(); + #endif + + static ubl_state state; + + static float z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y]; + + // 15 is the maximum nubmer of grid points supported + 1 safety margin for now, + // until determinism prevails + constexpr static float _mesh_index_to_xpos[16] PROGMEM = { + UBL_MESH_MIN_X + 0 * (MESH_X_DIST), UBL_MESH_MIN_X + 1 * (MESH_X_DIST), + UBL_MESH_MIN_X + 2 * (MESH_X_DIST), UBL_MESH_MIN_X + 3 * (MESH_X_DIST), + UBL_MESH_MIN_X + 4 * (MESH_X_DIST), UBL_MESH_MIN_X + 5 * (MESH_X_DIST), + UBL_MESH_MIN_X + 6 * (MESH_X_DIST), UBL_MESH_MIN_X + 7 * (MESH_X_DIST), + UBL_MESH_MIN_X + 8 * (MESH_X_DIST), UBL_MESH_MIN_X + 9 * (MESH_X_DIST), + UBL_MESH_MIN_X + 10 * (MESH_X_DIST), UBL_MESH_MIN_X + 11 * (MESH_X_DIST), + UBL_MESH_MIN_X + 12 * (MESH_X_DIST), UBL_MESH_MIN_X + 13 * (MESH_X_DIST), + UBL_MESH_MIN_X + 14 * (MESH_X_DIST), UBL_MESH_MIN_X + 15 * (MESH_X_DIST) + }; + + constexpr static float _mesh_index_to_ypos[16] PROGMEM = { + UBL_MESH_MIN_Y + 0 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 1 * (MESH_Y_DIST), + UBL_MESH_MIN_Y + 2 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 3 * (MESH_Y_DIST), + UBL_MESH_MIN_Y + 4 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 5 * (MESH_Y_DIST), + UBL_MESH_MIN_Y + 6 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 7 * (MESH_Y_DIST), + UBL_MESH_MIN_Y + 8 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 9 * (MESH_Y_DIST), + UBL_MESH_MIN_Y + 10 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 11 * (MESH_Y_DIST), + UBL_MESH_MIN_Y + 12 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 13 * (MESH_Y_DIST), + UBL_MESH_MIN_Y + 14 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 15 * (MESH_Y_DIST) + }; + + static bool g26_debug_flag, has_control_of_lcd_panel; + + static volatile int encoder_diff; // Volatile because it's changed at interrupt time. + + unified_bed_leveling(); + + FORCE_INLINE static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; } + + static int8_t get_cell_index_x(const float &x) { + const int8_t cx = (x - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST)); + return constrain(cx, 0, (GRID_MAX_POINTS_X) - 1); // -1 is appropriate if we want all movement to the X_MAX + } // position. But with this defined this way, it is possible + // to extrapolate off of this point even further out. Probably + // that is OK because something else should be keeping that from + // happening and should not be worried about at this level. + static int8_t get_cell_index_y(const float &y) { + const int8_t cy = (y - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_Y_DIST)); + return constrain(cy, 0, (GRID_MAX_POINTS_Y) - 1); // -1 is appropriate if we want all movement to the Y_MAX + } // position. But with this defined this way, it is possible + // to extrapolate off of this point even further out. Probably + // that is OK because something else should be keeping that from + // happening and should not be worried about at this level. + + static int8_t find_closest_x_index(const float &x) { + const int8_t px = (x - (UBL_MESH_MIN_X) + (MESH_X_DIST) * 0.5) * (1.0 / (MESH_X_DIST)); + return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1; + } + + static int8_t find_closest_y_index(const float &y) { + const int8_t py = (y - (UBL_MESH_MIN_Y) + (MESH_Y_DIST) * 0.5) * (1.0 / (MESH_Y_DIST)); + return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1; + } + + /** + * z2 --| + * z0 | | + * | | + (z2-z1) + * z1 | | | + * ---+-------------+--------+-- --| + * a1 a0 a2 + * |<---delta_a---------->| + * + * calc_z0 is the basis for all the Mesh Based correction. It is used to + * find the expected Z Height at a position between two known Z-Height locations. + * + * It is fairly expensive with its 4 floating point additions and 2 floating point + * multiplications. + */ + FORCE_INLINE static float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) { + return z1 + (z2 - z1) * (a0 - a1) / (a2 - a1); + } + + /** + * z_correction_for_x_on_horizontal_mesh_line is an optimization for + * the case where the printer is making a vertical line that only crosses horizontal mesh lines. + */ + inline static float z_correction_for_x_on_horizontal_mesh_line(const float &lx0, const int x1_i, const int yi) { + if (!WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 2) || !WITHIN(yi, 0, GRID_MAX_POINTS_Y - 1)) { + serialprintPGM( !WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1) ? PSTR("x1l_i") : PSTR("yi") ); + SERIAL_ECHOPAIR(" out of bounds in z_correction_for_x_on_horizontal_mesh_line(lx0=", lx0); + SERIAL_ECHOPAIR(",x1_i=", x1_i); + SERIAL_ECHOPAIR(",yi=", yi); + SERIAL_CHAR(')'); + SERIAL_EOL(); + return NAN; + } + + const float xratio = (RAW_X_POSITION(lx0) - mesh_index_to_xpos(x1_i)) * (1.0 / (MESH_X_DIST)), + z1 = z_values[x1_i][yi]; + + return z1 + xratio * (z_values[x1_i + 1][yi] - z1); + } + + // + // See comments above for z_correction_for_x_on_horizontal_mesh_line + // + inline static float z_correction_for_y_on_vertical_mesh_line(const float &ly0, const int xi, const int y1_i) { + if (!WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(y1_i, 0, GRID_MAX_POINTS_Y - 2)) { + serialprintPGM( !WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) ? PSTR("xi") : PSTR("yl_i") ); + SERIAL_ECHOPAIR(" out of bounds in z_correction_for_y_on_vertical_mesh_line(ly0=", ly0); + SERIAL_ECHOPAIR(", xi=", xi); + SERIAL_ECHOPAIR(", y1_i=", y1_i); + SERIAL_CHAR(')'); + SERIAL_EOL(); + return NAN; + } + + const float yratio = (RAW_Y_POSITION(ly0) - mesh_index_to_ypos(y1_i)) * (1.0 / (MESH_Y_DIST)), + z1 = z_values[xi][y1_i]; + + return z1 + yratio * (z_values[xi][y1_i + 1] - z1); + } + + /** + * This is the generic Z-Correction. It works anywhere within a Mesh Cell. It first + * does a linear interpolation along both of the bounding X-Mesh-Lines to find the + * Z-Height at both ends. Then it does a linear interpolation of these heights based + * on the Y position within the cell. + */ + static float get_z_correction(const float &lx0, const float &ly0) { + const int8_t cx = get_cell_index_x(RAW_X_POSITION(lx0)), + cy = get_cell_index_y(RAW_Y_POSITION(ly0)); + + if (!WITHIN(cx, 0, GRID_MAX_POINTS_X - 2) || !WITHIN(cy, 0, GRID_MAX_POINTS_Y - 2)) { + + SERIAL_ECHOPAIR("? in get_z_correction(lx0=", lx0); + SERIAL_ECHOPAIR(", ly0=", ly0); + SERIAL_CHAR(')'); + SERIAL_EOL(); + + #if ENABLED(ULTRA_LCD) + strcpy(lcd_status_message, "get_z_correction() indexes out of range."); + lcd_quick_feedback(); + #endif + return NAN; // this used to return state.z_offset + } + + const float z1 = calc_z0(RAW_X_POSITION(lx0), + mesh_index_to_xpos(cx), z_values[cx][cy], + mesh_index_to_xpos(cx + 1), z_values[cx + 1][cy]); + + const float z2 = calc_z0(RAW_X_POSITION(lx0), + mesh_index_to_xpos(cx), z_values[cx][cy + 1], + mesh_index_to_xpos(cx + 1), z_values[cx + 1][cy + 1]); + + float z0 = calc_z0(RAW_Y_POSITION(ly0), + mesh_index_to_ypos(cy), z1, + mesh_index_to_ypos(cy + 1), z2); + + #if ENABLED(DEBUG_LEVELING_FEATURE) + if (DEBUGGING(MESH_ADJUST)) { + SERIAL_ECHOPAIR(" raw get_z_correction(", lx0); + SERIAL_CHAR(','); + SERIAL_ECHO(ly0); + SERIAL_ECHOPGM(") = "); + SERIAL_ECHO_F(z0, 6); + } #endif - static float measure_point_with_encoder(); - static float measure_business_card_thickness(float); - static bool g29_parameter_parsing(); - static void find_mean_mesh_height(); - static void shift_mesh_height(); - static void probe_entire_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map, const bool stow_probe, bool do_furthest); - static void manually_probe_remaining_mesh(const float&, const float&, const float&, const float&, const bool); - static void tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3); - static void tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map); - static void g29_what_command(); - static void g29_eeprom_dump(); - static void g29_compare_current_mesh_to_stored_mesh(); - static void fine_tune_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map); - static bool smart_fill_one(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir); - static void smart_fill_mesh(); - - #if ENABLED(UBL_G26_MESH_VALIDATION) - static bool exit_from_g26(); - static bool parse_G26_parameters(); - static void G26_line_to_destination(const float &feed_rate); - static mesh_index_pair find_closest_circle_to_print(const float&, const float&); - static bool look_for_lines_to_connect(); - static bool turn_on_heaters(); - static bool prime_nozzle(); - static void retract_filament(const float where[XYZE]); - static void recover_filament(const float where[XYZE]); - static void print_line_from_here_to_there(const float&, const float&, const float&, const float&, const float&, const float&); - static void move_to(const float&, const float&, const float&, const float&); - inline static void move_to(const float where[XYZE], const float &de) { move_to(where[X_AXIS], where[Y_AXIS], where[Z_AXIS], de); } + #if ENABLED(DEBUG_LEVELING_FEATURE) + if (DEBUGGING(MESH_ADJUST)) { + SERIAL_ECHOPGM(" >>>---> "); + SERIAL_ECHO_F(z0, 6); + SERIAL_EOL(); + } #endif - public: - - static void echo_name(); - static void report_state(); - static void save_ubl_active_state_and_disable(); - static void restore_ubl_active_state_and_leave(); - static void display_map(const int); - static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, uint16_t[16], bool); - static void reset(); - static void invalidate(); - static void set_all_mesh_points_to_value(float); - static bool sanity_check(); - - static void G29() _O0; // O0 for no optimization - static void smart_fill_wlsf(const float &) _O2; // O2 gives smaller code than Os on A2560 - - #if ENABLED(UBL_G26_MESH_VALIDATION) - static void G26(); - #endif - - static ubl_state state; - - static float z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y]; - - // 15 is the maximum nubmer of grid points supported + 1 safety margin for now, - // until determinism prevails - constexpr static float _mesh_index_to_xpos[16] PROGMEM = { - UBL_MESH_MIN_X + 0 * (MESH_X_DIST), UBL_MESH_MIN_X + 1 * (MESH_X_DIST), - UBL_MESH_MIN_X + 2 * (MESH_X_DIST), UBL_MESH_MIN_X + 3 * (MESH_X_DIST), - UBL_MESH_MIN_X + 4 * (MESH_X_DIST), UBL_MESH_MIN_X + 5 * (MESH_X_DIST), - UBL_MESH_MIN_X + 6 * (MESH_X_DIST), UBL_MESH_MIN_X + 7 * (MESH_X_DIST), - UBL_MESH_MIN_X + 8 * (MESH_X_DIST), UBL_MESH_MIN_X + 9 * (MESH_X_DIST), - UBL_MESH_MIN_X + 10 * (MESH_X_DIST), UBL_MESH_MIN_X + 11 * (MESH_X_DIST), - UBL_MESH_MIN_X + 12 * (MESH_X_DIST), UBL_MESH_MIN_X + 13 * (MESH_X_DIST), - UBL_MESH_MIN_X + 14 * (MESH_X_DIST), UBL_MESH_MIN_X + 15 * (MESH_X_DIST) - }; - - constexpr static float _mesh_index_to_ypos[16] PROGMEM = { - UBL_MESH_MIN_Y + 0 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 1 * (MESH_Y_DIST), - UBL_MESH_MIN_Y + 2 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 3 * (MESH_Y_DIST), - UBL_MESH_MIN_Y + 4 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 5 * (MESH_Y_DIST), - UBL_MESH_MIN_Y + 6 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 7 * (MESH_Y_DIST), - UBL_MESH_MIN_Y + 8 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 9 * (MESH_Y_DIST), - UBL_MESH_MIN_Y + 10 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 11 * (MESH_Y_DIST), - UBL_MESH_MIN_Y + 12 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 13 * (MESH_Y_DIST), - UBL_MESH_MIN_Y + 14 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 15 * (MESH_Y_DIST) - }; - - static bool g26_debug_flag, has_control_of_lcd_panel; - - static volatile int encoder_diff; // Volatile because it's changed at interrupt time. - - unified_bed_leveling(); - - FORCE_INLINE static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; } - - static int8_t get_cell_index_x(const float &x) { - const int8_t cx = (x - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST)); - return constrain(cx, 0, (GRID_MAX_POINTS_X) - 1); // -1 is appropriate if we want all movement to the X_MAX - } // position. But with this defined this way, it is possible - // to extrapolate off of this point even further out. Probably - // that is OK because something else should be keeping that from - // happening and should not be worried about at this level. - static int8_t get_cell_index_y(const float &y) { - const int8_t cy = (y - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_Y_DIST)); - return constrain(cy, 0, (GRID_MAX_POINTS_Y) - 1); // -1 is appropriate if we want all movement to the Y_MAX - } // position. But with this defined this way, it is possible - // to extrapolate off of this point even further out. Probably - // that is OK because something else should be keeping that from - // happening and should not be worried about at this level. - - static int8_t find_closest_x_index(const float &x) { - const int8_t px = (x - (UBL_MESH_MIN_X) + (MESH_X_DIST) * 0.5) * (1.0 / (MESH_X_DIST)); - return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1; - } - - static int8_t find_closest_y_index(const float &y) { - const int8_t py = (y - (UBL_MESH_MIN_Y) + (MESH_Y_DIST) * 0.5) * (1.0 / (MESH_Y_DIST)); - return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1; - } - - /** - * z2 --| - * z0 | | - * | | + (z2-z1) - * z1 | | | - * ---+-------------+--------+-- --| - * a1 a0 a2 - * |<---delta_a---------->| - * - * calc_z0 is the basis for all the Mesh Based correction. It is used to - * find the expected Z Height at a position between two known Z-Height locations. - * - * It is fairly expensive with its 4 floating point additions and 2 floating point - * multiplications. - */ - FORCE_INLINE static float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) { - return z1 + (z2 - z1) * (a0 - a1) / (a2 - a1); - } - - /** - * z_correction_for_x_on_horizontal_mesh_line is an optimization for - * the case where the printer is making a vertical line that only crosses horizontal mesh lines. - */ - inline static float z_correction_for_x_on_horizontal_mesh_line(const float &lx0, const int x1_i, const int yi) { - if (!WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 2) || !WITHIN(yi, 0, GRID_MAX_POINTS_Y - 1)) { - serialprintPGM( !WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1) ? PSTR("x1l_i") : PSTR("yi") ); - SERIAL_ECHOPAIR(" out of bounds in z_correction_for_x_on_horizontal_mesh_line(lx0=", lx0); - SERIAL_ECHOPAIR(",x1_i=", x1_i); - SERIAL_ECHOPAIR(",yi=", yi); - SERIAL_CHAR(')'); - SERIAL_EOL(); - return NAN; - } - - const float xratio = (RAW_X_POSITION(lx0) - mesh_index_to_xpos(x1_i)) * (1.0 / (MESH_X_DIST)), - z1 = z_values[x1_i][yi]; - - return z1 + xratio * (z_values[x1_i + 1][yi] - z1); - } - - // - // See comments above for z_correction_for_x_on_horizontal_mesh_line - // - inline static float z_correction_for_y_on_vertical_mesh_line(const float &ly0, const int xi, const int y1_i) { - if (!WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(y1_i, 0, GRID_MAX_POINTS_Y - 2)) { - serialprintPGM( !WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) ? PSTR("xi") : PSTR("yl_i") ); - SERIAL_ECHOPAIR(" out of bounds in z_correction_for_y_on_vertical_mesh_line(ly0=", ly0); - SERIAL_ECHOPAIR(", xi=", xi); - SERIAL_ECHOPAIR(", y1_i=", y1_i); - SERIAL_CHAR(')'); - SERIAL_EOL(); - return NAN; - } - - const float yratio = (RAW_Y_POSITION(ly0) - mesh_index_to_ypos(y1_i)) * (1.0 / (MESH_Y_DIST)), - z1 = z_values[xi][y1_i]; - - return z1 + yratio * (z_values[xi][y1_i + 1] - z1); - } - - /** - * This is the generic Z-Correction. It works anywhere within a Mesh Cell. It first - * does a linear interpolation along both of the bounding X-Mesh-Lines to find the - * Z-Height at both ends. Then it does a linear interpolation of these heights based - * on the Y position within the cell. - */ - static float get_z_correction(const float &lx0, const float &ly0) { - const int8_t cx = get_cell_index_x(RAW_X_POSITION(lx0)), - cy = get_cell_index_y(RAW_Y_POSITION(ly0)); - - if (!WITHIN(cx, 0, GRID_MAX_POINTS_X - 2) || !WITHIN(cy, 0, GRID_MAX_POINTS_Y - 2)) { - - SERIAL_ECHOPAIR("? in get_z_correction(lx0=", lx0); - SERIAL_ECHOPAIR(", ly0=", ly0); - SERIAL_CHAR(')'); - SERIAL_EOL(); - - #if ENABLED(ULTRA_LCD) - strcpy(lcd_status_message, "get_z_correction() indexes out of range."); - lcd_quick_feedback(); - #endif - return NAN; // this used to return state.z_offset - } - - const float z1 = calc_z0(RAW_X_POSITION(lx0), - mesh_index_to_xpos(cx), z_values[cx][cy], - mesh_index_to_xpos(cx + 1), z_values[cx + 1][cy]); - - const float z2 = calc_z0(RAW_X_POSITION(lx0), - mesh_index_to_xpos(cx), z_values[cx][cy + 1], - mesh_index_to_xpos(cx + 1), z_values[cx + 1][cy + 1]); - - float z0 = calc_z0(RAW_Y_POSITION(ly0), - mesh_index_to_ypos(cy), z1, - mesh_index_to_ypos(cy + 1), z2); + if (isnan(z0)) { // if part of the Mesh is undefined, it will show up as NAN + z0 = 0.0; // in ubl.z_values[][] and propagate through the + // calculations. If our correction is NAN, we throw it out + // because part of the Mesh is undefined and we don't have the + // information we need to complete the height correction. #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(MESH_ADJUST)) { - SERIAL_ECHOPAIR(" raw get_z_correction(", lx0); + SERIAL_ECHOPAIR("??? Yikes! NAN in get_z_correction(", lx0); SERIAL_CHAR(','); SERIAL_ECHO(ly0); - SERIAL_ECHOPGM(") = "); - SERIAL_ECHO_F(z0, 6); - } - #endif - - #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(MESH_ADJUST)) { - SERIAL_ECHOPGM(" >>>---> "); - SERIAL_ECHO_F(z0, 6); + SERIAL_CHAR(')'); SERIAL_EOL(); } #endif + } + return z0; // there used to be a +state.z_offset on this line + } - if (isnan(z0)) { // if part of the Mesh is undefined, it will show up as NAN - z0 = 0.0; // in ubl.z_values[][] and propagate through the - // calculations. If our correction is NAN, we throw it out - // because part of the Mesh is undefined and we don't have the - // information we need to complete the height correction. - - #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(MESH_ADJUST)) { - SERIAL_ECHOPAIR("??? Yikes! NAN in get_z_correction(", lx0); - SERIAL_CHAR(','); - SERIAL_ECHO(ly0); - SERIAL_CHAR(')'); - SERIAL_EOL(); - } - #endif + /** + * This function sets the Z leveling fade factor based on the given Z height, + * only re-calculating when necessary. + * + * Returns 1.0 if planner.z_fade_height is 0.0. + * Returns 0.0 if Z is past the specified 'Fade Height'. + */ + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) + static inline float fade_scaling_factor_for_z(const float &lz) { + if (planner.z_fade_height == 0.0) return 1.0; + static float fade_scaling_factor = 1.0; + const float rz = RAW_Z_POSITION(lz); + if (last_specified_z != rz) { + last_specified_z = rz; + fade_scaling_factor = + rz < planner.z_fade_height + ? 1.0 - (rz * planner.inverse_z_fade_height) + : 0.0; } - return z0; // there used to be a +state.z_offset on this line + return fade_scaling_factor; } + #else + FORCE_INLINE static float fade_scaling_factor_for_z(const float &lz) { return 1.0; } + #endif - /** - * This function sets the Z leveling fade factor based on the given Z height, - * only re-calculating when necessary. - * - * Returns 1.0 if planner.z_fade_height is 0.0. - * Returns 0.0 if Z is past the specified 'Fade Height'. - */ - #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) - static inline float fade_scaling_factor_for_z(const float &lz) { - if (planner.z_fade_height == 0.0) return 1.0; - static float fade_scaling_factor = 1.0; - const float rz = RAW_Z_POSITION(lz); - if (last_specified_z != rz) { - last_specified_z = rz; - fade_scaling_factor = - rz < planner.z_fade_height - ? 1.0 - (rz * planner.inverse_z_fade_height) - : 0.0; - } - return fade_scaling_factor; - } - #else - FORCE_INLINE static float fade_scaling_factor_for_z(const float &lz) { return 1.0; } - #endif + FORCE_INLINE static float mesh_index_to_xpos(const uint8_t i) { + return i < GRID_MAX_POINTS_X ? pgm_read_float(&_mesh_index_to_xpos[i]) : UBL_MESH_MIN_X + i * (MESH_X_DIST); + } - FORCE_INLINE static float mesh_index_to_xpos(const uint8_t i) { - return i < GRID_MAX_POINTS_X ? pgm_read_float(&_mesh_index_to_xpos[i]) : UBL_MESH_MIN_X + i * (MESH_X_DIST); - } + FORCE_INLINE static float mesh_index_to_ypos(const uint8_t i) { + return i < GRID_MAX_POINTS_Y ? pgm_read_float(&_mesh_index_to_ypos[i]) : UBL_MESH_MIN_Y + i * (MESH_Y_DIST); + } - FORCE_INLINE static float mesh_index_to_ypos(const uint8_t i) { - return i < GRID_MAX_POINTS_Y ? pgm_read_float(&_mesh_index_to_ypos[i]) : UBL_MESH_MIN_Y + i * (MESH_Y_DIST); - } + static bool prepare_segmented_line_to(const float ltarget[XYZE], const float &feedrate); + static void line_to_destination_cartesian(const float &fr, uint8_t e); - static bool prepare_segmented_line_to(const float ltarget[XYZE], const float &feedrate); - static void line_to_destination_cartesian(const float &fr, uint8_t e); +}; // class unified_bed_leveling - }; // class unified_bed_leveling +extern unified_bed_leveling ubl; - extern unified_bed_leveling ubl; - - #if ENABLED(UBL_G26_MESH_VALIDATION) - FORCE_INLINE void gcode_G26() { ubl.G26(); } - #endif - - FORCE_INLINE void gcode_G29() { ubl.G29(); } - -#endif // AUTO_BED_LEVELING_UBL #endif // UNIFIED_BED_LEVELING_H diff --git a/Marlin/src/feature/ubl/ubl_G29.cpp b/Marlin/src/feature/ubl/ubl_G29.cpp index 95fa2494b..827c9c451 100644 --- a/Marlin/src/feature/ubl/ubl_G29.cpp +++ b/Marlin/src/feature/ubl/ubl_G29.cpp @@ -20,21 +20,22 @@ * */ -#include "MarlinConfig.h" +#include "../../inc/MarlinConfig.h" #if ENABLED(AUTO_BED_LEVELING_UBL) #include "ubl.h" - #include "Marlin.h" - #include "hex_print_routines.h" - #include "configuration_store.h" - #include "ultralcd.h" - #include "stepper.h" - #include "planner.h" - #include "gcode.h" + + #include "../../Marlin.h" + #include "../../libs/hex_print_routines.h" + #include "../../module/configuration_store.h" + #include "../../lcd/ultralcd.h" + #include "../../module/stepper.h" + #include "../../module/planner.h" + #include "../../gcode/parser.h" + #include "../../libs/least_squares_fit.h" #include - #include "least_squares_fit.h" #define UBL_G29_P31 diff --git a/Marlin/src/feature/ubl/ubl_motion.cpp b/Marlin/src/feature/ubl/ubl_motion.cpp index 13684b701..8157a3574 100644 --- a/Marlin/src/feature/ubl/ubl_motion.cpp +++ b/Marlin/src/feature/ubl/ubl_motion.cpp @@ -19,14 +19,16 @@ * along with this program. If not, see . * */ -#include "MarlinConfig.h" +#include "../../inc/MarlinConfig.h" #if ENABLED(AUTO_BED_LEVELING_UBL) - #include "Marlin.h" #include "ubl.h" - #include "planner.h" - #include "stepper.h" + + #include "../../Marlin.h" + #include "../../module/planner.h" + #include "../../module/stepper.h" + #include extern float destination[XYZE];