diff options
Diffstat (limited to 'Marlin/src/feature/encoder_i2c.cpp')
| -rw-r--r-- | Marlin/src/feature/encoder_i2c.cpp | 1139 |
1 files changed, 1139 insertions, 0 deletions
diff --git a/Marlin/src/feature/encoder_i2c.cpp b/Marlin/src/feature/encoder_i2c.cpp new file mode 100644 index 0000000..fa3cf15 --- /dev/null +++ b/Marlin/src/feature/encoder_i2c.cpp @@ -0,0 +1,1139 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 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 <https://www.gnu.org/licenses/>. + * + */ + +//todo: add support for multiple encoders on a single axis +//todo: add z axis auto-leveling +//todo: consolidate some of the related M codes? +//todo: add endstop-replacement mode? +//todo: try faster I2C speed; tweak TWI_FREQ (400000L, or faster?); or just TWBR = ((CPU_FREQ / 400000L) - 16) / 2; +//todo: consider Marlin-optimized Wire library; i.e. MarlinWire, like MarlinSerial + + +#include "../inc/MarlinConfig.h" + +#if ENABLED(I2C_POSITION_ENCODERS) + +#include "encoder_i2c.h" + +#include "../module/stepper.h" +#include "../gcode/parser.h" + +#include "../feature/babystep.h" + +#include <Wire.h> + +I2CPositionEncodersMgr I2CPEM; + +void I2CPositionEncoder::init(const uint8_t address, const AxisEnum axis) { + encoderAxis = axis; + i2cAddress = address; + + initialized++; + + SERIAL_ECHOLNPAIR("Setting up encoder on ", axis_codes[encoderAxis], " axis, addr = ", address); + + position = get_position(); +} + +void I2CPositionEncoder::update() { + if (!initialized || !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_ECHOLMPAIR("Fault detected on ", axis_codes[encoderAxis], " axis encoder. Disengaging error correction until module is trusted again."); + } + */ + 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. + */ + + #if 0 + // If the magnetic strength has been good for a certain time, start trusting the module again + + if (millis() - lastErrorTime > I2CPE_TIME_TRUSTED) { + trusted = true; + + SERIAL_ECHOLNPAIR("Untrusted encoder module on ", axis_codes[encoderAxis], " 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-initialize + const float pos = planner.get_axis_position_mm(encoderAxis); + int32_t positionInTicks = pos * get_ticks_unit(); + + //shift position from previous to current position + zeroOffset -= (positionInTicks - get_position()); + + #ifdef I2CPE_DEBUG + SERIAL_ECHOLNPAIR("Current position is ", pos); + SERIAL_ECHOLNPAIR("Position in encoder ticks is ", positionInTicks); + SERIAL_ECHOLNPAIR("New zero-offset of ", zeroOffset); + SERIAL_ECHOPAIR("New position reads as ", get_position()); + SERIAL_CHAR('('); + SERIAL_DECIMAL(mm_from_count(get_position())); + SERIAL_ECHOLNPGM(")"); + #endif + } + #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_ECHOLNPAIR("Axis error steps: ", error); + + #ifdef I2CPE_ERR_THRESH_ABORT + if (ABS(error) > I2CPE_ERR_THRESH_ABORT * planner.settings.axis_steps_per_mm[encoderAxis]) { + //kill(PSTR("Significant Error")); + SERIAL_ECHOLNPAIR("Axis error over threshold, aborting!", 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 (ABS(error) > threshold * planner.settings.axis_steps_per_mm[encoderAxis] + && diffSum < 10 * (I2CPE_ERR_ARRAY_SIZE - 1) + && ABS(error) < 2000 + ) { // Check for persistent error (skip) + errPrst[errPrstIdx++] = error; // Error must persist for I2CPE_ERR_PRST_ARRAY_SIZE error cycles. This also serves to improve the average accuracy + if (errPrstIdx >= I2CPE_ERR_PRST_ARRAY_SIZE) { + float sumP = 0; + LOOP_L_N(i, I2CPE_ERR_PRST_ARRAY_SIZE) sumP += errPrst[i]; + const int32_t errorP = int32_t(sumP * RECIPROCAL(I2CPE_ERR_PRST_ARRAY_SIZE)); + SERIAL_ECHO(axis_codes[encoderAxis]); + SERIAL_ECHOLNPAIR(" : CORRECT ERR ", errorP * planner.steps_to_mm[encoderAxis], "mm"); + babystep.add_steps(encoderAxis, -LROUND(errorP)); + errPrstIdx = 0; + } + } + else + errPrstIdx = 0; + } + #else + if (ABS(error) > threshold * planner.settings.axis_steps_per_mm[encoderAxis]) { + //SERIAL_ECHOLN(error); + //SERIAL_ECHOLN(position); + babystep.add_steps(encoderAxis, -LROUND(error / 2)); + } + #endif + + if (ABS(error) > I2CPE_ERR_CNT_THRESH * planner.settings.axis_steps_per_mm[encoderAxis]) { + const millis_t ms = millis(); + if (ELAPSED(ms, nextErrorCountTime)) { + SERIAL_ECHO(axis_codes[encoderAxis]); + SERIAL_ECHOLNPAIR(" : LARGE ERR ", int(error), "; diffSum=", diffSum); + errorCount++; + nextErrorCountTime = ms + I2CPE_ERR_CNT_DEBOUNCE_MS; + } + } + } + + 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_ECHOLNPAIR(" axis encoder homed, offset of ", zeroOffset, " ticks."); + #endif + } +} + +void I2CPositionEncoder::set_unhomed() { + zeroOffset = 0; + homed = trusted = false; + + #ifdef I2CPE_DEBUG + SERIAL_ECHO(axis_codes[encoderAxis]); + SERIAL_ECHOLNPGM(" axis encoder unhomed."); + #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_ternary(H == I2CPE_MAG_SIG_BAD, PSTR(" axis "), PSTR("magnetic strip "), PSTR("encoder ")); + switch (H) { + case I2CPE_MAG_SIG_GOOD: + case I2CPE_MAG_SIG_MID: + SERIAL_ECHO_TERNARY(H == I2CPE_MAG_SIG_GOOD, "passes test; field strength ", "good", "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) { + const float target = planner.get_axis_position_mm(encoderAxis), + actual = mm_from_count(position), + diff = actual - target, + error = ABS(diff) > 10000 ? 0 : diff; // Huge error is a bad reading + + if (report) { + SERIAL_ECHO(axis_codes[encoderAxis]); + SERIAL_ECHOLNPAIR(" axis target=", target, "mm; actual=", actual, "mm; err=", error, "mm"); + } + + return error; +} + +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); + + // 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.settings.axis_steps_per_mm[encoderAxis]; + + //convert both 'ticks' into same units / base + encoderCountInStepperTicksScaled = LROUND((stepperTicksPerUnit * encoderTicks) / encoderTicksPerUnit); + + const int32_t target = stepper.position(encoderAxis); + int32_t error = encoderCountInStepperTicksScaled - target; + + //suppress discontinuities (might be caused by bad I2C readings...?) + const bool suppressOutput = (ABS(error - errorPrev) > 100); + + errorPrev = error; + + if (report) { + SERIAL_ECHO(axis_codes[encoderAxis]); + SERIAL_ECHOLNPAIR(" axis target=", target, "; actual=", encoderCountInStepperTicksScaled, "; err=", error); + } + + if (suppressOutput) { + if (report) SERIAL_ECHOLNPGM("!Discontinuity. Suppressing error."); + error = 0; + } + + return error; +} + +int32_t I2CPositionEncoder::get_raw_count() { + uint8_t index = 0; + i2cLong encoderCount; + + encoderCount.val = 0x00; + + if (Wire.requestFrom(I2C_ADDRESS(i2cAddress), uint8_t(3)) != 3) { + //houston, we have a problem... + H = I2CPE_MAG_SIG_NF; + return 0; + } + + while (Wire.available()) + encoderCount.bval[index++] = (uint8_t)Wire.read(); + + //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; +} + +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; + + const float startPosition = soft_endstop.min[encoderAxis] + 10, + endPosition = soft_endstop.max[encoderAxis] - 10; + const feedRate_t fr_mm_s = FLOOR(homing_feedrate(encoderAxis)); + + ec = false; + + xyze_pos_t startCoord, endCoord; + LOOP_XYZ(a) { + startCoord[a] = planner.get_axis_position_mm((AxisEnum)a); + endCoord[a] = planner.get_axis_position_mm((AxisEnum)a); + } + startCoord[encoderAxis] = startPosition; + endCoord[encoderAxis] = endPosition; + + planner.synchronize(); + startCoord.e = planner.get_axis_position_mm(E_AXIS); + planner.buffer_line(startCoord, fr_mm_s, 0); + planner.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 + endCoord.e = planner.get_axis_position_mm(E_AXIS); + planner.buffer_line(endCoord, fr_mm_s, 0); + planner.synchronize(); + } + + return trusted; +} + +void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) { + if (type != I2CPE_ENC_TYPE_LINEAR) { + SERIAL_ECHOLNPGM("Steps/mm calibration requires linear encoder."); + return; + } + + if (!(encoderAxis == X_AXIS || encoderAxis == Y_AXIS || encoderAxis == Z_AXIS)) { + SERIAL_ECHOLNPGM("Steps/mm calibration not supported for this axis."); + return; + } + + float old_steps_mm, new_steps_mm, + startDistance, endDistance, + travelDistance, travelledDistance, total = 0; + + int32_t startCount, stopCount; + + const feedRate_t fr_mm_s = homing_feedrate(encoderAxis); + + bool oldec = ec; + ec = false; + + startDistance = 20; + endDistance = soft_endstop.max[encoderAxis] - 20; + travelDistance = endDistance - startDistance; + + xyze_pos_t startCoord, endCoord; + LOOP_XYZ(a) { + startCoord[a] = planner.get_axis_position_mm((AxisEnum)a); + endCoord[a] = planner.get_axis_position_mm((AxisEnum)a); + } + startCoord[encoderAxis] = startDistance; + endCoord[encoderAxis] = endDistance; + + planner.synchronize(); + + LOOP_L_N(i, iter) { + startCoord.e = planner.get_axis_position_mm(E_AXIS); + planner.buffer_line(startCoord, fr_mm_s, 0); + planner.synchronize(); + + delay(250); + startCount = get_position(); + + //do_blocking_move_to(endCoord); + + endCoord.e = planner.get_axis_position_mm(E_AXIS); + planner.buffer_line(endCoord, fr_mm_s, 0); + planner.synchronize(); + + //Read encoder distance + delay(250); + stopCount = get_position(); + + travelledDistance = mm_from_count(ABS(stopCount - startCount)); + + SERIAL_ECHOLNPAIR("Attempted travel: ", travelDistance, "mm"); + SERIAL_ECHOLNPAIR(" Actual travel: ", travelledDistance, "mm"); + + //Calculate new axis steps per unit + old_steps_mm = planner.settings.axis_steps_per_mm[encoderAxis]; + new_steps_mm = (old_steps_mm * travelDistance) / travelledDistance; + + SERIAL_ECHOLNPAIR("Old steps/mm: ", old_steps_mm); + SERIAL_ECHOLNPAIR("New steps/mm: ", new_steps_mm); + + //Save new value + planner.settings.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 + const float tempCoord = startCoord[encoderAxis]; + startCoord[encoderAxis] = endCoord[encoderAxis]; + endCoord[encoderAxis] = tempCoord; + } + } + + if (iter > 1) { + total /= (float)iter; + SERIAL_ECHOLNPAIR("Average steps/mm: ", total); + } + + ec = oldec; + + SERIAL_ECHOLNPGM("Calculated steps/mm set. Use M500 to save to EEPROM."); +} + +void I2CPositionEncoder::reset() { + Wire.beginTransmission(I2C_ADDRESS(i2cAddress)); + Wire.write(I2CPE_RESET_COUNT); + Wire.endTransmission(); + + TERN_(I2CPE_ERR_ROLLING_AVERAGE, ZERO(err)); +} + + +bool I2CPositionEncodersMgr::I2CPE_anyaxis; +uint8_t I2CPositionEncodersMgr::I2CPE_addr, + I2CPositionEncodersMgr::I2CPE_idx; +I2CPositionEncoder I2CPositionEncodersMgr::encoders[I2CPE_ENCODER_CNT]; + +void I2CPositionEncodersMgr::init() { + Wire.begin(); + + #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 + #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 + + encoders[i].set_active(encoders[i].passes_test(true)); + + #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 + + encoders[i].set_active(encoders[i].passes_test(true)); + + #if I2CPE_ENC_2_AXIS == E_AXIS + encoders[i].set_homed(); + #endif + #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 + #endif + + #if I2CPE_ENCODER_CNT > 3 + i++; + + encoders[i].init(I2CPE_ENC_4_ADDR, I2CPE_ENC_4_AXIS); + + #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 + + encoders[i].set_active(encoders[i].passes_test(true)); + + #if I2CPE_ENC_4_AXIS == E_AXIS + encoders[i].set_homed(); + #endif + #endif + + #if I2CPE_ENCODER_CNT > 4 + i++; + + 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 + #endif + + #if I2CPE_ENCODER_CNT > 5 + i++; + + encoders[i].init(I2CPE_ENC_6_ADDR, I2CPE_ENC_6_AXIS); + + #ifdef I2CPE_ENC_6_TYPE + encoders[i].set_type(I2CPE_ENC_6_TYPE); + #endif + #ifdef I2CPE_ENC_6_TICKS_UNIT + encoders[i].set_ticks_unit(I2CPE_ENC_6_TICKS_UNIT); + #endif + #ifdef I2CPE_ENC_6_TICKS_REV + encoders[i].set_stepper_ticks(I2CPE_ENC_6_TICKS_REV); + #endif + #ifdef I2CPE_ENC_6_INVERT + encoders[i].set_inverted(I2CPE_ENC_6_INVERT); + #endif + #ifdef I2CPE_ENC_6_EC_METHOD + encoders[i].set_ec_method(I2CPE_ENC_6_EC_METHOD); + #endif + #ifdef I2CPE_ENC_6_EC_THRESH + encoders[i].set_ec_threshold(I2CPE_ENC_6_EC_THRESH); + #endif + + encoders[i].set_active(encoders[i].passes_test(true)); + + #if I2CPE_ENC_6_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 (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(' '); + + 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(I2C_ADDRESS(newaddr)); + if (!Wire.endTransmission()) { + SERIAL_ECHOLNPAIR("?There is already a device with that address on the I2C bus! (", newaddr, ")"); + return; + } + + // Now check that we can find the module on the oldaddr address + Wire.beginTransmission(I2C_ADDRESS(oldaddr)); + if (Wire.endTransmission()) { + SERIAL_ECHOLNPAIR("?No module detected at this address! (", oldaddr, ")"); + return; + } + + SERIAL_ECHOLNPAIR("Module found at ", oldaddr, ", changing address to ", newaddr); + + // Change the modules address + Wire.beginTransmission(I2C_ADDRESS(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(I2C_ADDRESS(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 initialization 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(I2C_ADDRESS(address)); + if (Wire.endTransmission()) { + SERIAL_ECHOLNPAIR("?No module detected at this address! (", address, ")"); + return; + } + + SERIAL_ECHOLNPAIR("Requesting version info from module at address ", address, ":"); + + Wire.beginTransmission(I2C_ADDRESS(address)); + Wire.write(I2CPE_SET_REPORT_MODE); + Wire.write(I2CPE_REPORT_VERSION); + Wire.endTransmission(); + + // Read value + if (Wire.requestFrom(I2C_ADDRESS(address), uint8_t(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(I2C_ADDRESS(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_ECHOLNPGM("?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_ECHOLNPGM("?Address out of range. [30-200]"); + return I2CPE_PARSE_ERR; + } + + I2CPE_idx = idx_from_addr(I2CPE_addr); + if (I2CPE_idx >= I2CPE_ENCODER_CNT) { + SERIAL_ECHOLNPGM("?No device with this address!"); + return I2CPE_PARSE_ERR; + } + } + else if (parser.seenval('I')) { + + if (!parser.has_value()) { + SERIAL_ECHOLNPAIR("?I seen, but no index specified! [0-", I2CPE_ENCODER_CNT - 1, "]"); + return I2CPE_PARSE_ERR; + }; + + I2CPE_idx = parser.value_byte(); + if (I2CPE_idx >= I2CPE_ENCODER_CNT) { + SERIAL_ECHOLNPAIR("?Index out of range. [0-", I2CPE_ENCODER_CNT - 1, "]"); + 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<addr> Module I2C address. [30, 200]. + * I<index> 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<addr> Module I2C address. [30, 200]. + * I<index> 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<addr> Module I2C address. [30, 200]. + * I<index> 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<addr> Module I2C address. [30, 200]. + * I<index> 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<addr> Module current/old I2C address. If not present, + * assumes default address (030). [30, 200]. + * S<addr> 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_ECHOLNPGM("?S seen, but no address specified! [30-200]"); + return; + }; + + newAddress = parser.value_byte(); + if (!WITHIN(newAddress, 30, 200)) { + SERIAL_ECHOLNPGM("?New address out of range. [30-200]"); + return; + } + } + else if (!I2CPE_anyaxis) { + SERIAL_ECHOLNPGM("?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_ECHOLNPAIR("Changing module at address ", I2CPE_addr, " to address ", newAddress); + + change_module_address(I2CPE_addr, newAddress); +} + +/** + * M865: Check position encoder module firmware version. + * + * A<addr> Module I2C address. [30, 200]. + * I<index> 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<addr> Module I2C address. [30, 200]. + * I<index> 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<addr> Module I2C address. [30, 200]. + * I<index> 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<addr> Module I2C address. [30, 200]. + * I<index> 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<addr> Module I2C address. [30, 200]. + * I<index> 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); +} + +#endif // I2C_POSITION_ENCODERS |