12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145 |
- /**
- * Marlin 3D Printer Firmware
- * Copyright (c) 2019 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 <http://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 "I2CPositionEncoder.h"
-
- #include "../module/temperature.h"
- #include "../module/stepper.h"
- #include "../gcode/parser.h"
-
- #include "../feature/babystep.h"
-
- #include <Wire.h>
-
- 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 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_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_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(" - err detected: ", errorP * planner.steps_to_mm[encoderAxis], "mm; correcting!");
- 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_ECHOLNPAIR("Large error on ", axis_codes[encoderAxis], " axis. error: ", (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) {
- float target, actual, error;
-
- target = planner.get_axis_position_mm(encoderAxis);
- actual = mm_from_count(position);
- error = actual - target;
-
- if (ABS(error) > 10000) error = 0; // ?
-
- if (report) {
- SERIAL_ECHO(axis_codes[encoderAxis]);
- SERIAL_ECHOLNPAIR(" axis target: ", target, ", actual: ", actual, ", error : ",error);
- }
-
- 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);
-
- int32_t target = stepper.position(encoderAxis),
- error = (encoderCountInStepperTicksScaled - target);
-
- //suppress discontinuities (might be caused by bad I2C readings...?)
- const bool suppressOutput = (ABS(error - errorPrev) > 100);
-
- if (report) {
- SERIAL_ECHO(axis_codes[encoderAxis]);
- SERIAL_ECHOLNPAIR(" axis target: ", target, ", actual: ", encoderCountInStepperTicksScaled, ", error : ", error);
- if (suppressOutput) SERIAL_ECHOLNPGM("Discontinuity detected, suppressing error.");
- }
-
- errorPrev = error;
-
- return (suppressOutput ? 0 : error);
- }
-
- int32_t I2CPositionEncoder::get_raw_count() {
- uint8_t index = 0;
- i2cLong encoderCount;
-
- encoderCount.val = 0x00;
-
- if (Wire.requestFrom((int)i2cAddress, 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;
-
- float startCoord[NUM_AXIS] = { 0 }, endCoord[NUM_AXIS] = { 0 };
-
- const float startPosition = soft_endstop[encoderAxis].min + 10,
- endPosition = soft_endstop[encoderAxis].max - 10,
- feedrate = FLOOR(MMM_TO_MMS((encoderAxis == Z_AXIS) ? HOMING_FEEDRATE_Z : HOMING_FEEDRATE_XY));
-
- ec = false;
-
- LOOP_XYZ(i) {
- startCoord[i] = planner.get_axis_position_mm((AxisEnum)i);
- endCoord[i] = planner.get_axis_position_mm((AxisEnum)i);
- }
- startCoord[encoderAxis] = startPosition;
- endCoord[encoderAxis] = endPosition;
-
- planner.synchronize();
-
- planner.buffer_line(startCoord[X_AXIS], startCoord[Y_AXIS], startCoord[Z_AXIS],
- planner.get_axis_position_mm(E_AXIS), feedrate, 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
- planner.buffer_line(endCoord[X_AXIS], endCoord[Y_AXIS], endCoord[Z_AXIS],
- planner.get_axis_position_mm(E_AXIS), feedrate, 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,
- 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[encoderAxis].max - 20;
- travelDistance = endDistance - startDistance;
-
- 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) {
- planner.buffer_line(startCoord[X_AXIS], startCoord[Y_AXIS], startCoord[Z_AXIS],
- planner.get_axis_position_mm(E_AXIS), feedrate, 0);
- planner.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],
- planner.get_axis_position_mm(E_AXIS), feedrate, 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();
-
- #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE)
- ZERO(err);
- #endif
- }
-
-
- 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((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(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
|