[New Feature] I2C position encoder support (#6946)

* [New Feature] I2C position encoder support

I plan to continue improving/cleaning this up, as there areas that need work.

* let the cleanups begin.

* progress

* more progress

* comments, rename files, etc.

* clean

* Cleanups per thinkyhead

* a few more cleanups

* cleanups, bugfixes, etc.

* remove unnecessary passes_test(), additional cleanups/optimizations

* cleanups

* misc.

* Fix up I2CPEM.init() and a few other things.

* organize, fix, rename, etc.

* more optimization

* a few more tweaks

Marlin/Configuration_adv.h

@@ -1261,4 +1261,87 @@
   #define USER_GCODE_5 "G28\nM503"
 #endif
 
+//===========================================================================
+//============================ I2C Encoder Settings =========================
+//===========================================================================
+/**
+ *  I2C position encoders for closed loop control.
+ *  Developed by Chris Barr at Aus3D.
+ *
+ *  Wiki: http://wiki.aus3d.com.au/Magnetic_Encoder
+ *  Github: https://github.com/Aus3D/MagneticEncoder
+ *
+ *  Supplier: http://aus3d.com.au/magnetic-encoder-module
+ *  Alternative Supplier: http://reliabuild3d.com/
+ *
+ *  Reilabuild encoders have been modified to improve reliability.
+ */
+
+//#define I2C_POSITION_ENCODERS
+#if ENABLED(I2C_POSITION_ENCODERS)
+
+  #define I2CPE_ENCODER_CNT         1                       // The number of encoders installed; max of 5
+                                                            // encoders supported currently.
+
+  #define I2CPE_ENC_1_ADDR          I2CPE_PRESET_ADDR_X     // I2C address of the encoder. 30-200.
+  #define I2CPE_ENC_1_AXIS          X_AXIS                  // Axis the encoder module is installed on.  <X|Y|Z|E>_AXIS.
+  #define I2CPE_ENC_1_TYPE          I2CPE_ENC_TYPE_LINEAR   // Type of encoder:  I2CPE_ENC_TYPE_LINEAR -or-
+                                                            // I2CPE_ENC_TYPE_ROTARY.
+  #define I2CPE_ENC_1_TICKS_UNIT    2048                    // 1024 for magnetic strips with 2mm poles; 2048 for
+                                                            // 1mm poles. For linear encoders this is ticks / mm,
+                                                            // for rotary encoders this is ticks / revolution.
+  //#define I2CPE_ENC_1_TICKS_REV     (16 * 200)            // Only needed for rotary encoders; number of stepper
+                                                            // steps per full revolution (motor steps/rev * microstepping)
+  //#define I2CPE_ENC_1_INVERT                              // Invert the direction of axis travel.
+  #define I2CPE_ENC_1_EC_METHOD     I2CPE_ECM_NONE          // Type of error error correction.
+  #define I2CPE_ENC_1_EC_THRESH     0.10                    // Threshold size for error (in mm) above which the
+                                                            // printer will attempt to correct the error; errors
+                                                            // smaller than this are ignored to minimize effects of
+                                                            // measurement noise / latency (filter).
+
+  #define I2CPE_ENC_2_ADDR          I2CPE_PRESET_ADDR_Y     // Same as above, but for encoder 2.
+  #define I2CPE_ENC_2_AXIS          Y_AXIS
+  #define I2CPE_ENC_2_TYPE          I2CPE_ENC_TYPE_LINEAR
+  #define I2CPE_ENC_2_TICKS_UNIT    2048
+  //#define I2CPE_ENC_2_TICKS_REV   (16 * 200)
+  //#define I2CPE_ENC_2_INVERT
+  #define I2CPE_ENC_2_EC_METHOD     I2CPE_ECM_NONE
+  #define I2CPE_ENC_2_EC_THRESH     0.10
+
+  #define I2CPE_ENC_3_ADDR          I2CPE_PRESET_ADDR_Z     // Encoder 3.  Add additional configuration options
+  #define I2CPE_ENC_3_AXIS          Z_AXIS                  // as above, or use defaults below.
+
+  #define I2CPE_ENC_4_ADDR          I2CPE_PRESET_ADDR_E     // Encoder 4.
+  #define I2CPE_ENC_4_AXIS          E_AXIS
+
+  #define I2CPE_ENC_5_ADDR          34                      // Encoder 5.
+  #define I2CPE_ENC_5_AXIS          E_AXIS
+
+  // Default settings for encoders which are enabled, but without settings configured above.
+  #define I2CPE_DEF_TYPE            I2CPE_ENC_TYPE_LINEAR
+  #define I2CPE_DEF_ENC_TICKS_UNIT  2048
+  #define I2CPE_DEF_TICKS_REV       (16 * 200)
+  #define I2CPE_DEF_EC_METHOD       I2CPE_ECM_NONE
+  #define I2CPE_DEF_EC_THRESH       0.1
+
+  //#define I2CPE_ERR_THRESH_ABORT  100.0                   // Threshold size for error (in mm) error on any given
+                                                            // axis after which the printer will abort. Comment out to
+                                                            // disable abort behaviour.
+
+  #define I2CPE_TIME_TRUSTED        10000                   // After an encoder fault, there must be no further fault
+                                                            // for this amount of time (in ms) before the encoder
+                                                            // is trusted again.
+
+  /**
+   * Position is checked every time a new command is executed from the buffer but during long moves,
+   * this setting determines the minimum update time between checks. A value of 100 works well with
+   * error rolling average when attempting to correct only for skips and not for vibration.
+   */
+  #define I2CPE_MIN_UPD_TIME_MS     100                     // Minimum time in miliseconds between encoder checks.
+
+  // Use a rolling average to identify persistant errors that indicate skips, as opposed to vibration and noise.
+  #define I2CPE_ERR_ROLLING_AVERAGE
+
+#endif
+
 #endif // CONFIGURATION_ADV_H

Marlin/I2CPositionEncoder.h

@@ -0,0 +1,356 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016, 2017 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/>.
+ *
+ */
+
+#ifndef I2CPOSENC_H
+#define I2CPOSENC_H
+
+#include "MarlinConfig.h"
+
+#if ENABLED(I2C_POSITION_ENCODERS)
+
+  #include "enum.h"
+  #include "macros.h"
+  #include "types.h"
+  #include <Wire.h>
+
+  //=========== 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_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_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_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
+
+  // 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
+
+  // 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
+
+  // 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
+
+  // 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 if (!WITHIN(idx, 0, I2CPE_ENCODER_CNT - 1)) return;
+
+  extern const char axis_codes[XYZE];
+
+  typedef union {
+    volatile long   val = 0;
+    uint8_t         bval[4];
+  } i2cLong;
+
+  class I2CPositionEncoder {
+  private:
+    AxisEnum        encoderAxis             = I2CPE_DEF_AXIS;
+
+    uint8_t         i2cAddress              = I2CPE_DEF_ADDR,
+                    ecMethod                = I2CPE_DEF_EC_METHOD,
+                    type                    = I2CPE_DEF_TYPE,
+                    H                       = I2CPE_MAG_SIG_NF;    // Magnetic field strength
+
+    int             encoderTicksPerUnit     = I2CPE_DEF_ENC_TICKS_UNIT,
+                    stepperTicks            = I2CPE_DEF_TICKS_REV;
+
+    float           ecThreshold             = I2CPE_DEF_EC_THRESH;
+
+    bool            homed                   = false,
+                    trusted                 = false,
+                    initialised             = false,
+                    active                  = false,
+                    invert                  = false,
+                    ec                      = true;
+
+    int             errorCount              = 0,
+                    errorPrev               = 0;
+
+    float           axisOffset              = 0;
+
+    long            axisOffsetTicks         = 0,
+                    zeroOffset              = 0,
+                    lastPosition            = 0,
+                    position;
+
+    unsigned long   lastPositionTime        = 0,
+                    lastErrorCountTime      = 0,
+                    lastErrorTime;
+
+    //double        positionMm; //calculate
+
+    #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE)
+      uint8_t       errIdx = 0;
+      int           err[I2CPE_ERR_ARRAY_SIZE] = {0};
+    #endif
+
+  public:
+    void init(uint8_t address, AxisEnum axis);
+    void reset();
+
+    void update();
+
+    void set_homed();
+
+    long get_raw_count();
+
+    FORCE_INLINE double mm_from_count(long count) {
+      if (type == I2CPE_ENC_TYPE_LINEAR) return count / encoderTicksPerUnit;
+      else if (type == I2CPE_ENC_TYPE_ROTARY)
+        return (count * stepperTicks) / (encoderTicksPerUnit * planner.axis_steps_per_mm[encoderAxis]);
+      return -1;
+    }
+
+    FORCE_INLINE double get_position_mm() { return mm_from_count(get_position()); }
+    FORCE_INLINE long get_position() { return get_raw_count() - zeroOffset - axisOffsetTicks; }
+
+    long get_axis_error_steps(bool report);
+    double get_axis_error_mm(bool report);
+
+    void calibrate_steps_mm(int iter);
+
+    bool passes_test(bool report);
+
+    bool test_axis(void);
+
+    FORCE_INLINE int get_error_count(void) { return errorCount; }
+    FORCE_INLINE void set_error_count(int newCount) { errorCount = newCount; }
+
+    FORCE_INLINE uint8_t get_address() { return i2cAddress; }
+    FORCE_INLINE void set_address(uint8_t addr) { i2cAddress = addr; }
+
+    FORCE_INLINE bool get_active(void) { return active; }
+    FORCE_INLINE void set_active(bool a) { active = a; }
+
+    FORCE_INLINE void set_inverted(bool i) { invert = i; }
+
+    FORCE_INLINE AxisEnum get_axis() { return encoderAxis; }
+
+    FORCE_INLINE bool get_ec_enabled() { return ec; }
+    FORCE_INLINE void set_ec_enabled(bool enabled) { ec = enabled; }
+
+    FORCE_INLINE uint8_t get_ec_method() { return ecMethod; }
+    FORCE_INLINE void set_ec_method(byte method) { ecMethod = method; }
+
+    FORCE_INLINE float get_ec_threshold() { return ecThreshold; }
+    FORCE_INLINE void set_ec_threshold(float newThreshold) { ecThreshold = newThreshold; }
+
+    FORCE_INLINE int get_encoder_ticks_mm() {
+      if (type == I2CPE_ENC_TYPE_LINEAR) return encoderTicksPerUnit;
+      else if (type == I2CPE_ENC_TYPE_ROTARY)
+        return (int)((encoderTicksPerUnit / stepperTicks) * planner.axis_steps_per_mm[encoderAxis]);
+      return 0;
+    }
+
+    FORCE_INLINE int get_ticks_unit() { return encoderTicksPerUnit; }
+    FORCE_INLINE void set_ticks_unit(int ticks) { encoderTicksPerUnit = ticks; }
+
+    FORCE_INLINE uint8_t get_type() { return type; }
+    FORCE_INLINE void set_type(byte newType) { type = newType; }
+
+    FORCE_INLINE int get_stepper_ticks() { return stepperTicks; }
+    FORCE_INLINE void set_stepper_ticks(int ticks) { stepperTicks = ticks; }
+
+    FORCE_INLINE float get_axis_offset() { return axisOffset; }
+    FORCE_INLINE void set_axis_offset(float newOffset) {
+      axisOffset = newOffset;
+      axisOffsetTicks = (long)(axisOffset * get_encoder_ticks_mm());
+    }
+
+    FORCE_INLINE void set_current_position(float newPositionMm) {
+      set_axis_offset(get_position_mm() - newPositionMm + axisOffset);
+    }
+  };
+
+  class I2CPositionEncodersMgr {
+  private:
+    bool I2CPE_anyaxis;
+    uint8_t I2CPE_addr;
+    int8_t I2CPE_idx;
+
+  public:
+    void init(void);
+
+    // consider only updating one endoder per call / tick if encoders become too time intensive
+    void update(void) { LOOP_PE(i) encoders[i].update(); }
+
+    void homed(AxisEnum axis) {
+      LOOP_PE(i)
+        if (encoders[i].get_axis() == axis) encoders[i].set_homed();
+    }
+
+    void report_position(uint8_t idx, bool units, bool noOffset);
+
+    void report_status(uint8_t idx) {
+      CHECK_IDX
+      SERIAL_ECHOPAIR("Encoder ",idx);
+      SERIAL_ECHOPGM(": ");
+      encoders[idx].get_raw_count();
+      encoders[idx].passes_test(true);
+    }
+
+    void report_error(uint8_t idx) {
+      CHECK_IDX
+      encoders[idx].get_axis_error_steps(true);
+    }
+
+    void test_axis(uint8_t idx) {
+      CHECK_IDX
+      encoders[idx].test_axis();
+    }
+
+    void calibrate_steps_mm(uint8_t idx, int iterations) {
+      CHECK_IDX
+      encoders[idx].calibrate_steps_mm(iterations);
+    }
+
+    void change_module_address(uint8_t oldaddr, uint8_t newaddr);
+    void report_module_firmware(uint8_t address);
+
+    void report_error_count(uint8_t idx, AxisEnum axis) {
+      CHECK_IDX
+      SERIAL_ECHOPAIR("Error count on ", axis_codes[axis]);
+      SERIAL_ECHOLNPAIR(" axis is ", encoders[idx].get_error_count());
+    }
+
+    void reset_error_count(uint8_t idx, AxisEnum axis) {
+      CHECK_IDX
+      encoders[idx].set_error_count(0);
+      SERIAL_ECHOPAIR("Error count on ", axis_codes[axis]);
+      SERIAL_ECHOLNPGM(" axis has been reset.");
+    }
+
+    void enable_ec(uint8_t idx, bool enabled, AxisEnum axis) {
+      CHECK_IDX
+      encoders[idx].set_ec_enabled(enabled);
+      SERIAL_ECHOPAIR("Error correction on ", axis_codes[axis]);
+      SERIAL_ECHOPGM(" axis is ");
+      serialprintPGM(encoders[idx].get_ec_enabled() ? PSTR("en") : PSTR("dis"));
+      SERIAL_ECHOLNPGM("abled.");
+    }
+
+    void set_ec_threshold(uint8_t idx, float newThreshold, AxisEnum axis) {
+      CHECK_IDX
+      encoders[idx].set_ec_threshold(newThreshold);
+      SERIAL_ECHOPAIR("Error correct threshold for ", axis_codes[axis]);
+      SERIAL_ECHOPAIR_F(" axis set to ", newThreshold);
+      SERIAL_ECHOLNPGM("mm.");
+    }
+
+    void get_ec_threshold(uint8_t idx, AxisEnum axis) {
+      CHECK_IDX
+      float threshold = encoders[idx].get_ec_threshold();
+      SERIAL_ECHOPAIR("Error correct threshold for ", axis_codes[axis]);
+      SERIAL_ECHOPAIR_F(" axis is ", threshold);
+      SERIAL_ECHOLNPGM("mm.");
+    }
+
+    int8_t idx_from_axis(AxisEnum axis) {
+      LOOP_PE(i)
+        if (encoders[i].get_axis() == axis) return i;
+
+      return -1;
+    }
+
+    int8_t idx_from_addr(uint8_t addr) {
+      LOOP_PE(i)
+        if (encoders[i].get_address() == addr) return i;
+
+      return -1;
+    }
+
+    int8_t parse();
+
+    void M860();
+    void M861();
+    void M862();
+    void M863();
+    void M864();
+    void M865();
+    void M866();
+    void M867();
+    void M868();
+    void M869();
+
+    I2CPositionEncoder encoders[I2CPE_ENCODER_CNT];
+  };
+
+  extern I2CPositionEncodersMgr I2CPEM;
+
+  FORCE_INLINE void gcode_M860() { I2CPEM.M860(); }
+  FORCE_INLINE void gcode_M861() { I2CPEM.M861(); }
+  FORCE_INLINE void gcode_M862() { I2CPEM.M862(); }
+  FORCE_INLINE void gcode_M863() { I2CPEM.M863(); }
+  FORCE_INLINE void gcode_M864() { I2CPEM.M864(); }
+  FORCE_INLINE void gcode_M865() { I2CPEM.M865(); }
+  FORCE_INLINE void gcode_M866() { I2CPEM.M866(); }
+  FORCE_INLINE void gcode_M867() { I2CPEM.M867(); }
+  FORCE_INLINE void gcode_M868() { I2CPEM.M868(); }
+  FORCE_INLINE void gcode_M869() { I2CPEM.M869(); }
+
+#endif //I2C_POSITION_ENCODERS
+#endif //I2CPOSENC_H
+
+

Marlin/Marlin_main.cpp

@@ -200,6 +200,16 @@
  * M666 - Set delta endstop adjustment. (Requires DELTA)
  * M605 - Set dual x-carriage movement mode: "M605 S<mode> [X<x_offset>] [R<temp_offset>]". (Requires DUAL_X_CARRIAGE)
  * M851 - Set Z probe's Z offset in current units. (Negative = below the nozzle.)
+ * M860 - Report the position of position encoder modules.
+ * M861 - Report the status of position encoder modules.
+ * M862 - Perform an axis continuity test for position encoder modules.
+ * M863 - Perform steps-per-mm calibration for position encoder modules.
+ * M864 - Change position encoder module I2C address.
+ * M865 - Check position encoder module firmware version.
+ * M866 - Report or reset position encoder module error count.
+ * M867 - Enable/disable or toggle error correction for position encoder modules.
+ * M868 - Report or set position encoder module error correction threshold.
+ * M869 - Report position encoder module error.
  * M900 - Get and/or Set advance K factor and WH/D ratio. (Requires LIN_ADVANCE)
  * M906 - Set or get motor current in milliamps using axis codes X, Y, Z, E. Report values if no axis codes given. (Requires HAVE_TMC2130)
  * M907 - Set digital trimpot motor current using axis codes. (Requires a board with digital trimpots)
@@ -286,6 +296,10 @@
   #include "twibus.h"
 #endif
 
+#if ENABLED(I2C_POSITION_ENCODERS)
+  #include "I2CPositionEncoder.h"
+#endif
+
 #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
   #include "endstop_interrupts.h"
 #endif
@@ -662,6 +676,12 @@ static bool send_ok[BUFSIZE];
   #define host_keepalive() NOOP
 #endif
 
+#if ENABLED(I2C_POSITION_ENCODERS)
+  I2CPositionEncodersMgr I2CPEM;
+  uint8_t blockBufferIndexRef = 0;
+  millis_t lastUpdateMillis;
+#endif
+
 FORCE_INLINE float pgm_read_any(const float *p) { return pgm_read_float_near(p); }
 FORCE_INLINE signed char pgm_read_any(const signed char *p) { return pgm_read_byte_near(p); }
 
@@ -1493,6 +1513,10 @@ static void set_axis_is_at_home(const AxisEnum axis) {
       SERIAL_EOL;
     }
   #endif
+
+  #if ENABLED(I2C_POSITION_ENCODERS)
+    I2CPEM.homed(axis);
+  #endif
 }
 
 /**
@@ -5609,6 +5633,11 @@ inline void gcode_G92() {
           #if HAS_POSITION_SHIFT
             position_shift[i] += v - p; // Offset the coordinate space
             update_software_endstops((AxisEnum)i);
+
+            #if ENABLED(I2C_POSITION_ENCODERS)
+              I2CPEM.encoders[I2CPEM.idx_from_axis((AxisEnum) i)].set_axis_offset(position_shift[i]);
+            #endif
+
           #endif
         }
       #endif
@@ -10904,6 +10933,50 @@ void process_next_command() {
           break;
       #endif
 
+      #if ENABLED(I2C_POSITION_ENCODERS)
+
+        case 860: // M860 Report encoder module position
+          gcode_M860();
+          break;
+
+        case 861: // M861 Report encoder module status
+          gcode_M861();
+          break;
+
+        case 862: // M862 Perform axis test
+          gcode_M862();
+          break;
+
+        case 863: // M863 Calibrate steps/mm
+          gcode_M863();
+          break;
+
+        case 864: // M864 Change module address
+          gcode_M864();
+          break;
+
+        case 865: // M865 Check module firmware version
+          gcode_M865();
+          break;
+
+        case 866: // M866 Report axis error count
+          gcode_M866();
+          break;
+
+        case 867: // M867 Toggle error correction
+          gcode_M867();
+          break;
+
+        case 868: // M868 Set error correction threshold
+          gcode_M868();
+          break;
+
+        case 869: // M869 Report axis error
+          gcode_M869();
+          break;
+
+      #endif // I2C_POSITION_ENCODERS
+
       case 999: // M999: Restart after being Stopped
         gcode_M999();
         break;
@@ -12200,7 +12273,7 @@ void disable_all_steppers() {
       const bool has_days = (elapsed.value > 60*60*24L);
       (void)elapsed.toDigital(timestamp, has_days);
       SERIAL_ECHO(timestamp);
-      SERIAL_ECHO(": ");
+      SERIAL_ECHOPGM(": ");
       SERIAL_ECHO(axisID);
       SERIAL_ECHOLNPGM(" driver overtemperature warning!");
     }
@@ -12495,6 +12568,16 @@ void idle(
   #if HAS_BUZZER && DISABLED(LCD_USE_I2C_BUZZER)
     buzzer.tick();
   #endif
+
+  #if ENABLED(I2C_POSITION_ENCODERS)
+    if (planner.blocks_queued() &&
+        ( (blockBufferIndexRef != planner.block_buffer_head) ||
+          ((lastUpdateMillis + I2CPE_MIN_UPD_TIME_MS) < millis())) ) {
+      blockBufferIndexRef = planner.block_buffer_head;
+      I2CPEM.update();
+      lastUpdateMillis = millis();
+    }
+  #endif
 }
 
 /**
@@ -12739,6 +12822,10 @@ void setup() {
     set_bltouch_deployed(false);
   #endif
 
+  #if ENABLED(I2C_POSITION_ENCODERS)
+    I2CPEM.init();
+  #endif
+
   #if ENABLED(EXPERIMENTAL_I2CBUS) && I2C_SLAVE_ADDRESS > 0
     i2c.onReceive(i2c_on_receive);
     i2c.onRequest(i2c_on_request);

Marlin/SanityCheck.h

@@ -270,11 +270,24 @@
   #endif
 #endif
 
+/**
+ * I2C Position Encoders
+ */
+#if ENABLED(I2C_POSITION_ENCODERS)
+  #if DISABLED(BABYSTEPPING)
+    #error "I2C_POSITION_ENCODERS requires BABYSTEPPING."
+  #endif
+
+  #if I2CPE_ENCODER_CNT > 5 || I2CPE_ENCODER_CNT < 1
+    #error "I2CPE_ENCODER_CNT must be between 1 and 5."
+  #endif
+#endif
+
 /**
  * Babystepping
  */
 #if ENABLED(BABYSTEPPING)
-  #if DISABLED(ULTRA_LCD)
+  #if DISABLED(ULTRA_LCD) && DISABLED(I2C_POSITION_ENCODERS)
     #error "BABYSTEPPING requires an LCD controller."
   #elif ENABLED(SCARA)
     #error "BABYSTEPPING is not implemented for SCARA yet."

Marlin/enum.h

@@ -48,9 +48,15 @@ enum AxisEnum {
   ALL_AXES  = 100
 };
 
-#define LOOP_XYZ(VAR)  for (uint8_t VAR=X_AXIS; VAR<=Z_AXIS; VAR++)
+#define LOOP_S_LE_N(VAR, S, N) for (uint8_t VAR=S; VAR<=N; VAR++)
-#define LOOP_XYZE(VAR) for (uint8_t VAR=X_AXIS; VAR<=E_AXIS; VAR++)
+#define LOOP_S_L_N(VAR, S, N) for (uint8_t VAR=S; VAR<N; VAR++)
-#define LOOP_XYZE_N(VAR) for (uint8_t VAR=X_AXIS; VAR<XYZE_N; VAR++)
+#define LOOP_LE_N(VAR, N) LOOP_S_LE_N(VAR, 0, N)
+#define LOOP_L_N(VAR, N) LOOP_S_L_N(VAR, 0, N)
+
+#define LOOP_NA(VAR) LOOP_L_N(VAR, NUM_AXIS)
+#define LOOP_XYZ(VAR) LOOP_S_LE_N(VAR, X_AXIS, Z_AXIS)
+#define LOOP_XYZE(VAR) LOOP_S_LE_N(VAR, X_AXIS, E_AXIS)
+#define LOOP_XYZE_N(VAR) LOOP_S_L_N(VAR, X_AXIS, XYZE_N)
 
 typedef enum {
   LINEARUNIT_MM,

Marlin/gcode.h

@@ -128,6 +128,12 @@ public:
       return b;
     }
 
+    static volatile bool seen_any() {
+      return codebits[3] || codebits[2] || codebits[1] || codebits[0];
+    }
+
+    #define SEEN_TEST(L) TEST(codebits[(L - 'A') >> 3], (L - 'A') & 0x7)
+
   #else
 
     // Code is found in the string. If not found, value_ptr is unchanged.
@@ -139,6 +145,12 @@ public:
       return b;
     }
 
+    static volatile bool seen_any() {
+      return *command_args == '\0';
+    }
+
+    #define SEEN_TEST(L) !!strchr(command_args, L)
+
   #endif // FASTER_GCODE_PARSER
 
   // Populate all fields by parsing a single line of GCode
@@ -148,6 +160,13 @@ public:
   // Code value pointer was set
   FORCE_INLINE static bool has_value() { return value_ptr != NULL; }
 
+  // Seen and has value
+  FORCE_INLINE static bool seenval(const char c) { return seen(c) && has_value(); }
+
+  static volatile bool seen_axis() {
+    return SEEN_TEST('X') || SEEN_TEST('Y') || SEEN_TEST('Z') || SEEN_TEST('E');
+  }
+
   // Float removes 'E' to prevent scientific notation interpretation
   inline static float value_float() {
     if (value_ptr) {

Marlin/macros.h

@@ -108,6 +108,8 @@
 #define HYPOT2(x,y) (sq(x)+sq(y))
 #define HYPOT(x,y) sqrt(HYPOT2(x,y))
 
+#define SIGN(a) ((a>0)-(a<0))
+
 // Macros to contrain values
 #define NOLESS(v,n) do{ if (v < n) v = n; }while(0)
 #define NOMORE(v,n) do{ if (v > n) v = n; }while(0)