From fc140c49624ba845f68d0e2180b0086f585febc3 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Fri, 9 Jun 2017 10:22:49 -0500
Subject: [PATCH] Followup for I2C_POSITION_ENCODERS

---
 .travis.yml                   |   2 +-
 Marlin/Configuration_adv.h    |   1 +
 Marlin/I2CPositionEncoder.cpp | 420 ++++++++++++++++++----------------
 Marlin/I2CPositionEncoder.h   | 235 +++++++++----------
 Marlin/Marlin_main.cpp        |   4 +-
 Marlin/SanityCheck.h          |   4 +-
 Marlin/ubl_G29.cpp            |   7 +-
 7 files changed, 352 insertions(+), 321 deletions(-)

diff --git a/.travis.yml b/.travis.yml
index 4d5a00f1b2..f12b1e8918 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -93,7 +93,7 @@ script:
   #
   - restore_configs
   - opt_enable AUTO_BED_LEVELING_UBL UBL_G26_MESH_EDITING ENABLE_LEVELING_FADE_HEIGHT FIX_MOUNTED_PROBE EEPROM_SETTINGS G3D_PANEL
-  - opt_enable_adv CUSTOM_USER_MENUS
+  - opt_enable_adv CUSTOM_USER_MENUS I2C_POSITION_ENCODERS BABYSTEPPING
   - build_marlin
   #
   # Test a Sled Z Probe
diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index 4c772ec2d7..08d30b58e1 100644
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@@ -1271,6 +1271,7 @@
 //===========================================================================
 //====================== I2C Position Encoder Settings ======================
 //===========================================================================
+
 /**
  *  I2C position encoders for closed loop control.
  *  Developed by Chris Barr at Aus3D.
diff --git a/Marlin/I2CPositionEncoder.cpp b/Marlin/I2CPositionEncoder.cpp
index 8f226d9635..84334812f7 100644
--- a/Marlin/I2CPositionEncoder.cpp
+++ b/Marlin/I2CPositionEncoder.cpp
@@ -40,13 +40,14 @@
 
   #include <Wire.h>
 
-  void I2CPositionEncoder::init(uint8_t address, AxisEnum axis) {
+
+  void I2CPositionEncoder::init(const uint8_t address, const AxisEnum axis) {
     encoderAxis = axis;
     i2cAddress = address;
 
     initialised++;
 
-    SERIAL_ECHOPAIR("Seetting up encoder on ", axis_codes[encoderAxis]);
+    SERIAL_ECHOPAIR("Setting up encoder on ", axis_codes[encoderAxis]);
     SERIAL_ECHOLNPAIR(" axis, addr = ", address);
 
     position = get_position();
@@ -98,13 +99,13 @@
 
           //the encoder likely lost its place when the error occured, so we'll reset and use the printer's
           //idea of where it the axis is to re-initialise
-          double position = stepper.get_axis_position_mm(encoderAxis);
-          long positionInTicks = position * get_ticks_unit();
+          float position = stepper.get_axis_position_mm(encoderAxis);
+          int32_t positionInTicks = position * get_ticks_unit();
 
           //shift position from previous to current position
           zeroOffset -= (positionInTicks - get_position());
 
-          #if defined(I2CPE_DEBUG)
+          #ifdef I2CPE_DEBUG
             SERIAL_ECHOPGM("Current position is ");
             SERIAL_ECHOLN(position);
 
@@ -126,23 +127,23 @@
     }
 
     lastPosition = position;
-    millis_t positionTime = millis();
+    const millis_t positionTime = millis();
 
     //only do error correction if setup and enabled
     if (ec && ecMethod != I2CPE_ECM_NONE) {
 
-      #if defined(I2CPE_EC_THRESH_PROPORTIONAL)
-        millis_t deltaTime = positionTime - lastPositionTime;
-        unsigned long distance = abs(position - lastPosition);
-        unsigned long speed = distance / deltaTime;
-        float threshold = constrain(speed / 50, 1, 50) * ecThreshold;
+      #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
-        float threshold = get_error_correct_threshold();
+        const float threshold = get_error_correct_threshold();
       #endif
 
       //check error
       #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE)
-        double sum = 0, diffSum = 0;
+        float sum = 0, diffSum = 0;
 
         errIdx = (errIdx >= I2CPE_ERR_ARRAY_SIZE - 1) ? 0 : errIdx + 1;
         err[errIdx] = get_axis_error_steps(false);
@@ -152,16 +153,16 @@
           if (i) diffSum += abs(err[i-1] - err[i]);
         }
 
-        long error = (long)(sum/(I2CPE_ERR_ARRAY_SIZE + 1)); //calculate average for error
+        const int32_t error = int32_t(sum / (I2CPE_ERR_ARRAY_SIZE + 1)); //calculate average for error
 
       #else
-        long error = get_axis_error_steps(false);
+        const int32_t error = get_axis_error_steps(false);
       #endif
 
-      //SERIAL_ECHOPGM("Axis err*r steps: ");
+      //SERIAL_ECHOPGM("Axis error steps: ");
       //SERIAL_ECHOLN(error);
 
-      #if defined(I2CPE_ERR_THRESH_ABORT)
+      #ifdef I2CPE_ERR_THRESH_ABORT
         if (labs(error) > I2CPE_ERR_THRESH_ABORT * planner.axis_steps_per_mm[encoderAxis]) {
           //kill("Significant Error");
           SERIAL_ECHOPGM("Axis error greater than set threshold, aborting!");
@@ -215,7 +216,7 @@
       homed++;
       trusted++;
 
-      #if defined(I2CPE_DEBUG)
+      #ifdef I2CPE_DEBUG
         SERIAL_ECHO(axis_codes[encoderAxis]);
         SERIAL_ECHOPAIR(" axis encoder homed, offset of ", zeroOffset);
         SERIAL_ECHOLNPGM(" ticks.");
@@ -223,36 +224,27 @@
     }
   }
 
-  bool I2CPositionEncoder::passes_test(bool report) {
-    if (H == I2CPE_MAG_SIG_GOOD) {
-      if (report) {
-        SERIAL_ECHO(axis_codes[encoderAxis]);
-        SERIAL_ECHOLNPGM(" axis encoder passes test; field strength good.");
-      }
-      return true;
-    } else if (H == I2CPE_MAG_SIG_MID) {
-      if (report) {
-        SERIAL_ECHOPAIR("Warning, ", axis_codes[encoderAxis]);
-        SERIAL_ECHOLNPGM(" axis encoder passes test; field strength fair.");
-      }
-      return true;
-    } else if (H == I2CPE_MAG_SIG_BAD) {
-      if (report) {
-        SERIAL_ECHOPAIR("Warning, ", axis_codes[encoderAxis]);
-        SERIAL_ECHOLNPGM(" axis magnetic strip not detected!");
-      }
-      return false;
-    }
-
+  bool I2CPositionEncoder::passes_test(const bool report) {
     if (report) {
-      SERIAL_ECHOPAIR("Warning, ", axis_codes[encoderAxis]);
-      SERIAL_ECHOLNPGM(" axis encoder not detected!");
+      if (H != I2CPE_MAG_SIG_GOOD) SERIAL_ECHOPGM("Warning. ");
+      SERIAL_ECHO(axis_codes[encoderAxis]);
+      SERIAL_ECHOPGM(" axis ");
+      serialprintPGM(H == I2CPE_MAG_SIG_BAD ? PSTR("magnetic strip ") : PSTR("encoder "));
+      switch (H) {
+        case I2CPE_MAG_SIG_GOOD:
+        case I2CPE_MAG_SIG_MID:
+          SERIAL_ECHOLNPGM("passes test; field strength ");
+          serialprintPGM(H == I2CPE_MAG_SIG_GOOD ? PSTR("good.\n") : PSTR("fair.\n"));
+          break;
+        default:
+          SERIAL_ECHOLNPGM("not detected!");
+      }
     }
-    return false;
+    return (H == I2CPE_MAG_SIG_GOOD || H == I2CPE_MAG_SIG_MID);
   }
 
-  double I2CPositionEncoder::get_axis_error_mm(bool report) {
-    double target, actual, error;
+  float I2CPositionEncoder::get_axis_error_mm(const bool report) {
+    float target, actual, error;
 
     target = stepper.get_axis_position_mm(encoderAxis);
     actual = mm_from_count(position);
@@ -270,7 +262,7 @@
     return error;
   }
 
-  long I2CPositionEncoder::get_axis_error_steps(bool report) {
+  int32_t I2CPositionEncoder::get_axis_error_steps(const bool report) {
     if (!active) {
       if (report) {
         SERIAL_ECHO(axis_codes[encoderAxis]);
@@ -280,8 +272,8 @@
     }
 
     float stepperTicksPerUnit;
-    long encoderTicks = position, encoderCountInStepperTicksScaled;
-    //long stepperTicks = stepper.position(encoderAxis);
+    int32_t encoderTicks = position, encoderCountInStepperTicksScaled;
+    //int32_t stepperTicks = stepper.position(encoderAxis);
 
     // With a rotary encoder we're concerned with ticks/rev; whereas with a linear we're concerned with ticks/mm
     stepperTicksPerUnit = (type == I2CPE_ENC_TYPE_ROTARY) ? stepperTicks : planner.axis_steps_per_mm[encoderAxis];
@@ -289,8 +281,8 @@
     //convert both 'ticks' into same units / base
     encoderCountInStepperTicksScaled = LROUND((stepperTicksPerUnit * encoderTicks) / encoderTicksPerUnit);
 
-    long target = stepper.position(encoderAxis),
-         error = (encoderCountInStepperTicksScaled - target);
+    int32_t target = stepper.position(encoderAxis),
+            error = (encoderCountInStepperTicksScaled - target);
 
     //suppress discontinuities (might be caused by bad I2C readings...?)
     bool suppressOutput = (labs(error - errorPrev) > 100);
@@ -309,7 +301,7 @@
     return (suppressOutput ? 0 : error);
   }
 
-  long I2CPositionEncoder::get_raw_count() {
+  int32_t I2CPositionEncoder::get_raw_count() {
     uint8_t index = 0;
     i2cLong encoderCount;
 
@@ -340,14 +332,11 @@
     //only works on XYZ cartesian machines for the time being
     if (!(encoderAxis == X_AXIS || encoderAxis == Y_AXIS || encoderAxis == Z_AXIS)) return false;
 
-    int feedrate;
-    float startPosition, endPosition;
-    float startCoord[NUM_AXIS] = {0}, endCoord[NUM_AXIS] = {0};
+    float startCoord[NUM_AXIS] = { 0 }, endCoord[NUM_AXIS] = { 0 };
 
-    startPosition = soft_endstop_min[encoderAxis] + 10;
-    endPosition = soft_endstop_max[encoderAxis] - 10;
-
-    feedrate = (int)MMM_TO_MMS((encoderAxis == Z_AXIS) ? HOMING_FEEDRATE_Z : HOMING_FEEDRATE_XY);
+    const float startPosition = soft_endstop_min[encoderAxis] + 10,
+                endPosition = soft_endstop_max[encoderAxis] - 10,
+                feedrate = FLOOR(MMM_TO_MMS((encoderAxis == Z_AXIS) ? HOMING_FEEDRATE_Z : HOMING_FEEDRATE_XY));
 
     ec = false;
 
@@ -367,7 +356,7 @@
 
     // if the module isn't currently trusted, wait until it is (or until it should be if things are working)
     if (!trusted) {
-      long startWaitingTime = millis();
+      int32_t startWaitingTime = millis();
       while (!trusted && millis() - startWaitingTime < I2CPE_TIME_TRUSTED)
         safe_delay(500);
     }
@@ -381,7 +370,7 @@
     return trusted;
   }
 
-  void I2CPositionEncoder::calibrate_steps_mm(int iter) {
+  void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) {
     if (type != I2CPE_ENC_TYPE_LINEAR) {
       SERIAL_ECHOLNPGM("Steps per mm calibration is only available using linear encoders.");
       return;
@@ -392,14 +381,14 @@
       return;
     }
 
-    float oldStepsMm, newStepsMm,
+    float old_steps_mm, new_steps_mm,
           startDistance, endDistance,
           travelDistance, travelledDistance, total = 0,
-          startCoord[NUM_AXIS] = {0}, endCoord[NUM_AXIS] = {0};
+          startCoord[NUM_AXIS] = { 0 }, endCoord[NUM_AXIS] = { 0 };
 
-    double feedrate;
+    float feedrate;
 
-    long startCount, stopCount;
+    int32_t startCount, stopCount;
 
     feedrate = MMM_TO_MMS((encoderAxis == Z_AXIS) ? HOMING_FEEDRATE_Z : HOMING_FEEDRATE_XY);
 
@@ -447,17 +436,17 @@
       SERIAL_ECHOLNPGM("mm.");
 
       //Calculate new axis steps per unit
-      oldStepsMm = planner.axis_steps_per_mm[encoderAxis];
-      newStepsMm = (oldStepsMm * travelDistance) / travelledDistance;
+      old_steps_mm = planner.axis_steps_per_mm[encoderAxis];
+      new_steps_mm = (old_steps_mm * travelDistance) / travelledDistance;
 
-      SERIAL_ECHOLNPAIR("Old steps per mm: ", oldStepsMm);
-      SERIAL_ECHOLNPAIR("New steps per mm: ", newStepsMm);
+      SERIAL_ECHOLNPAIR("Old steps per mm: ", old_steps_mm);
+      SERIAL_ECHOLNPAIR("New steps per mm: ", new_steps_mm);
 
       //Save new value
-      planner.axis_steps_per_mm[encoderAxis] = newStepsMm;
+      planner.axis_steps_per_mm[encoderAxis] = new_steps_mm;
 
       if (iter > 1) {
-        total += newStepsMm;
+        total += new_steps_mm;
 
         // swap start and end points so next loop runs from current position
         float tempCoord = startCoord[encoderAxis];
@@ -486,6 +475,12 @@
     #endif
   }
 
+
+  bool I2CPositionEncodersMgr::I2CPE_anyaxis;
+  uint8_t I2CPositionEncodersMgr::I2CPE_addr,
+          I2CPositionEncodersMgr::I2CPE_idx;
+  I2CPositionEncoder I2CPositionEncodersMgr::encoders[I2CPE_ENCODER_CNT];
+
   void I2CPositionEncodersMgr::init() {
     Wire.begin();
 
@@ -494,28 +489,28 @@
 
       encoders[i].init(I2CPE_ENC_1_ADDR, I2CPE_ENC_1_AXIS);
 
-      #if defined(I2CPE_ENC_1_TYPE)
+      #ifdef I2CPE_ENC_1_TYPE
         encoders[i].set_type(I2CPE_ENC_1_TYPE);
       #endif
-      #if defined(I2CPE_ENC_1_TICKS_UNIT)
+      #ifdef I2CPE_ENC_1_TICKS_UNIT
         encoders[i].set_ticks_unit(I2CPE_ENC_1_TICKS_UNIT);
       #endif
-      #if defined(I2CPE_ENC_1_TICKS_REV)
+      #ifdef I2CPE_ENC_1_TICKS_REV
         encoders[i].set_stepper_ticks(I2CPE_ENC_1_TICKS_REV);
       #endif
-      #if defined(I2CPE_ENC_1_INVERT)
+      #ifdef I2CPE_ENC_1_INVERT
         encoders[i].set_inverted(I2CPE_ENC_1_INVERT);
       #endif
-      #if defined(I2CPE_ENC_1_EC_METHOD)
+      #ifdef I2CPE_ENC_1_EC_METHOD
         encoders[i].set_ec_method(I2CPE_ENC_1_EC_METHOD);
       #endif
-      #if defined(I2CPE_ENC_1_EC_THRESH)
+      #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)
+      #if I2CPE_ENC_1_AXIS == E_AXIS
         encoders[i].set_homed();
       #endif
     #endif
@@ -525,28 +520,28 @@
 
       encoders[i].init(I2CPE_ENC_2_ADDR, I2CPE_ENC_2_AXIS);
 
-      #if defined(I2CPE_ENC_2_TYPE)
+      #ifdef I2CPE_ENC_2_TYPE
         encoders[i].set_type(I2CPE_ENC_2_TYPE);
       #endif
-      #if defined(I2CPE_ENC_2_TICKS_UNIT)
+      #ifdef I2CPE_ENC_2_TICKS_UNIT
         encoders[i].set_ticks_unit(I2CPE_ENC_2_TICKS_UNIT);
       #endif
-      #if defined(I2CPE_ENC_2_TICKS_REV)
+      #ifdef I2CPE_ENC_2_TICKS_REV
         encoders[i].set_stepper_ticks(I2CPE_ENC_2_TICKS_REV);
       #endif
-      #if defined(I2CPE_ENC_2_INVERT)
+      #ifdef I2CPE_ENC_2_INVERT
         encoders[i].set_inverted(I2CPE_ENC_2_INVERT);
       #endif
-      #if defined(I2CPE_ENC_2_EC_METHOD)
+      #ifdef I2CPE_ENC_2_EC_METHOD
         encoders[i].set_ec_method(I2CPE_ENC_2_EC_METHOD);
       #endif
-      #if defined(I2CPE_ENC_2_EC_THRESH)
+      #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)
+      #if I2CPE_ENC_2_AXIS == E_AXIS
         encoders[i].set_homed();
       #endif
     #endif
@@ -556,28 +551,28 @@
 
       encoders[i].init(I2CPE_ENC_3_ADDR, I2CPE_ENC_3_AXIS);
 
-      #if defined(I2CPE_ENC_3_TYPE)
+      #ifdef I2CPE_ENC_3_TYPE
         encoders[i].set_type(I2CPE_ENC_3_TYPE);
       #endif
-      #if defined(I2CPE_ENC_3_TICKS_UNIT)
+      #ifdef I2CPE_ENC_3_TICKS_UNIT
         encoders[i].set_ticks_unit(I2CPE_ENC_3_TICKS_UNIT);
       #endif
-      #if defined(I2CPE_ENC_3_TICKS_REV)
+      #ifdef I2CPE_ENC_3_TICKS_REV
         encoders[i].set_stepper_ticks(I2CPE_ENC_3_TICKS_REV);
       #endif
-      #if defined(I2CPE_ENC_3_INVERT)
+      #ifdef I2CPE_ENC_3_INVERT
         encoders[i].set_inverted(I2CPE_ENC_3_INVERT);
       #endif
-      #if defined(I2CPE_ENC_3_EC_METHOD)
+      #ifdef I2CPE_ENC_3_EC_METHOD
         encoders[i].set_ec_method(I2CPE_ENC_3_EC_METHOD);
       #endif
-      #if defined(I2CPE_ENC_3_EC_THRESH)
+      #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)
+      #if I2CPE_ENC_3_AXIS == E_AXIS
         encoders[i].set_homed();
       #endif
     #endif
@@ -587,28 +582,28 @@
 
       encoders[i].init(I2CPE_ENC_4_ADDR, I2CPE_ENC_4_AXIS);
 
-      #if defined(I2CPE_ENC_4_TYPE)
+      #ifdef I2CPE_ENC_4_TYPE
         encoders[i].set_type(I2CPE_ENC_4_TYPE);
       #endif
-      #if defined(I2CPE_ENC_4_TICKS_UNIT)
+      #ifdef I2CPE_ENC_4_TICKS_UNIT
         encoders[i].set_ticks_unit(I2CPE_ENC_4_TICKS_UNIT);
       #endif
-      #if defined(I2CPE_ENC_4_TICKS_REV)
+      #ifdef I2CPE_ENC_4_TICKS_REV
         encoders[i].set_stepper_ticks(I2CPE_ENC_4_TICKS_REV);
       #endif
-      #if defined(I2CPE_ENC_4_INVERT)
+      #ifdef I2CPE_ENC_4_INVERT
         encoders[i].set_inverted(I2CPE_ENC_4_INVERT);
       #endif
-      #if defined(I2CPE_ENC_4_EC_METHOD)
+      #ifdef I2CPE_ENC_4_EC_METHOD
         encoders[i].set_ec_method(I2CPE_ENC_4_EC_METHOD);
       #endif
-      #if defined(I2CPE_ENC_4_EC_THRESH)
+      #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)
+      #if I2CPE_ENC_4_AXIS == E_AXIS
         encoders[i].set_homed();
       #endif
     #endif
@@ -618,56 +613,57 @@
 
       encoders[i].init(I2CPE_ENC_5_ADDR, I2CPE_ENC_5_AXIS);
 
-      #if defined(I2CPE_ENC_5_TYPE)
+      #ifdef I2CPE_ENC_5_TYPE
         encoders[i].set_type(I2CPE_ENC_5_TYPE);
       #endif
-      #if defined(I2CPE_ENC_5_TICKS_UNIT)
+      #ifdef I2CPE_ENC_5_TICKS_UNIT
         encoders[i].set_ticks_unit(I2CPE_ENC_5_TICKS_UNIT);
       #endif
-      #if defined(I2CPE_ENC_5_TICKS_REV)
+      #ifdef I2CPE_ENC_5_TICKS_REV
         encoders[i].set_stepper_ticks(I2CPE_ENC_5_TICKS_REV);
       #endif
-      #if defined(I2CPE_ENC_5_INVERT)
+      #ifdef I2CPE_ENC_5_INVERT
         encoders[i].set_inverted(I2CPE_ENC_5_INVERT);
       #endif
-      #if defined(I2CPE_ENC_5_EC_METHOD)
+      #ifdef I2CPE_ENC_5_EC_METHOD
         encoders[i].set_ec_method(I2CPE_ENC_5_EC_METHOD);
       #endif
-      #if defined(I2CPE_ENC_5_EC_THRESH)
+      #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)
+      #if I2CPE_ENC_5_AXIS == E_AXIS
         encoders[i].set_homed();
       #endif
     #endif
-
   }
 
-  void I2CPositionEncodersMgr::report_position(uint8_t idx, bool units, bool noOffset) {
-    CHECK_IDX
+  void I2CPositionEncodersMgr::report_position(const int8_t idx, const bool units, const bool noOffset) {
+    CHECK_IDX();
 
-    if (units) {
+    if (units)
       SERIAL_ECHOLN(noOffset ? encoders[idx].mm_from_count(encoders[idx].get_raw_count()) : encoders[idx].get_position_mm());
-    } else {
+    else {
       if (noOffset) {
-        long raw_count = encoders[idx].get_raw_count();
+        const int32_t raw_count = encoders[idx].get_raw_count();
         SERIAL_ECHO(axis_codes[encoders[idx].get_axis()]);
-        SERIAL_ECHOPGM(" ");
+        SERIAL_CHAR(' ');
 
         for (uint8_t j = 31; j > 0; j--)
           SERIAL_ECHO((bool)(0x00000001 & (raw_count >> j)));
 
-        SERIAL_ECHO((bool)(0x00000001 & (raw_count)));
-        SERIAL_ECHOLNPAIR(" ", raw_count);
-      } else
+        SERIAL_ECHO((bool)(0x00000001 & raw_count));
+        SERIAL_CHAR(' ');
+        SERIAL_ECHOLN(raw_count);
+      }
+      else
         SERIAL_ECHOLN(encoders[idx].get_position());
     }
   }
 
-  void I2CPositionEncodersMgr::change_module_address(uint8_t oldaddr, uint8_t newaddr) {
+  void I2CPositionEncodersMgr::change_module_address(const uint8_t oldaddr, const uint8_t newaddr) {
     // First check 'new' address is not in use
     Wire.beginTransmission(newaddr);
     if (!Wire.endTransmission()) {
@@ -709,7 +705,7 @@
 
     // Now, if this module is configured, find which encoder instance it's supposed to correspond to
     // and enable it (it will likely have failed initialisation on power-up, before the address change).
-    int8_t idx = idx_from_addr(newaddr);
+    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.");
@@ -717,7 +713,7 @@
     }
   }
 
-  void I2CPositionEncodersMgr::report_module_firmware(uint8_t address) {
+  void I2CPositionEncodersMgr::report_module_firmware(const uint8_t address) {
     // First check there is a module
     Wire.beginTransmission(address);
     if (Wire.endTransmission()) {
@@ -727,7 +723,7 @@
     }
 
     SERIAL_ECHOPAIR("Requesting version info from module at address ", address);
-    SERIAL_ECHOPGM(":\n");
+    SERIAL_ECHOLNPGM(":");
 
     Wire.beginTransmission(address);
     Wire.write(I2CPE_SET_REPORT_MODE);
@@ -743,7 +739,7 @@
     }
 
     // Set module back to normal (distance) mode
-    Wire.beginTransmission((int)address);
+    Wire.beginTransmission(address);
     Wire.write(I2CPE_SET_REPORT_MODE);
     Wire.write(I2CPE_REPORT_DISTANCE);
     Wire.endTransmission();
@@ -753,43 +749,43 @@
     I2CPE_addr = 0;
 
     if (parser.seen('A')) {
+
       if (!parser.has_value()) {
         SERIAL_PROTOCOLLNPGM("?A seen, but no address specified! [30-200]");
         return I2CPE_PARSE_ERR;
       };
 
       I2CPE_addr = parser.value_byte();
-
       if (!WITHIN(I2CPE_addr, 30, 200)) { // reserve the first 30 and last 55
         SERIAL_PROTOCOLLNPGM("?Address out of range. [30-200]");
         return I2CPE_PARSE_ERR;
       }
 
       I2CPE_idx = idx_from_addr(I2CPE_addr);
-
-      if (!WITHIN(I2CPE_idx, 0, I2CPE_ENCODER_CNT - 1)) {
+      if (I2CPE_idx >= I2CPE_ENCODER_CNT) {
         SERIAL_PROTOCOLLNPGM("?No device with this address!");
         return I2CPE_PARSE_ERR;
       }
-    } else if (parser.seenval('I')) {
+    }
+    else if (parser.seenval('I')) {
+
       if (!parser.has_value()) {
         SERIAL_PROTOCOLLNPAIR("?I seen, but no index specified! [0-", I2CPE_ENCODER_CNT - 1);
-        SERIAL_ECHOLNPGM("]");
+        SERIAL_PROTOCOLLNPGM("]");
         return I2CPE_PARSE_ERR;
       };
 
       I2CPE_idx = parser.value_byte();
-
-      if (!WITHIN(I2CPE_idx, 0, I2CPE_ENCODER_CNT - 1)) {
+      if (I2CPE_idx >= I2CPE_ENCODER_CNT) {
         SERIAL_PROTOCOLLNPAIR("?Index out of range. [0-", I2CPE_ENCODER_CNT - 1);
         SERIAL_ECHOLNPGM("]");
         return I2CPE_PARSE_ERR;
       }
 
       I2CPE_addr = encoders[I2CPE_idx].get_address();
-    } else {
-      I2CPE_idx = -1;
     }
+    else
+      I2CPE_idx = 0xFF;
 
     I2CPE_anyaxis = parser.seen_axis();
 
@@ -814,15 +810,18 @@
   void I2CPositionEncodersMgr::M860() {
     if (parse()) return;
 
-    bool hasU = parser.seen('U'), hasO = parser.seen('O');
+    const bool hasU = parser.seen('U'), hasO = parser.seen('O');
 
-    if (I2CPE_idx < 0) {
-      int8_t idx;
+    if (I2CPE_idx == 0xFF) {
       LOOP_XYZE(i) {
-        if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0))
-          report_position((uint8_t)idx, hasU, hasO);
+        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((uint8_t)I2CPE_idx, hasU, hasO);
+    }
+    else
+      report_position(I2CPE_idx, hasU, hasO);
   }
 
   /**
@@ -841,13 +840,16 @@
   void I2CPositionEncodersMgr::M861() {
     if (parse()) return;
 
-    if (I2CPE_idx < 0) {
-      int8_t idx;
+    if (I2CPE_idx == 0xFF) {
       LOOP_XYZE(i) {
-        if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0))
-          report_status((uint8_t)idx);
+        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((uint8_t)I2CPE_idx);
+    }
+    else
+      report_status(I2CPE_idx);
   }
 
   /**
@@ -867,13 +869,16 @@
   void I2CPositionEncodersMgr::M862() {
     if (parse()) return;
 
-    if (I2CPE_idx < 0) {
-      int8_t idx;
+    if (I2CPE_idx == 0xFF) {
       LOOP_XYZE(i) {
-        if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0))
-          test_axis((uint8_t)idx);
+        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((uint8_t)I2CPE_idx);
+    }
+    else
+      test_axis(I2CPE_idx);
   }
 
   /**
@@ -894,15 +899,18 @@
   void I2CPositionEncodersMgr::M863() {
     if (parse()) return;
 
-    int iterations = parser.seenval('P') ? constrain(parser.value_byte(), 1, 10) : 1;
+    const uint8_t iterations = constrain(parser.byteval('P', 1), 1, 10);
 
-    if (I2CPE_idx < 0) {
-      int8_t idx;
+    if (I2CPE_idx == 0xFF) {
       LOOP_XYZE(i) {
-        if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0))
-          calibrate_steps_mm((uint8_t)idx, iterations);
+        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((uint8_t)I2CPE_idx, iterations);
+    }
+    else
+      calibrate_steps_mm(I2CPE_idx, iterations);
   }
 
   /**
@@ -910,9 +918,9 @@
    *
    *   A<addr>  Module current/old I2C address.  If not present,
    *            assumes default address (030).  [30, 200].
-   *   N<addr>  Module new I2C address. [30, 200].
+   *   S<addr>  Module new I2C address. [30, 200].
    *
-   *   If N not specified:
+   *   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).
@@ -925,23 +933,24 @@
 
     if (!I2CPE_addr) I2CPE_addr = I2CPE_PRESET_ADDR_X;
 
-    if (parser.seen('N')) {
+    if (parser.seen('S')) {
       if (!parser.has_value()) {
-        SERIAL_PROTOCOLLNPGM("?N seen, but no address specified! [30-200]");
+        SERIAL_PROTOCOLLNPGM("?S seen, but no address specified! [30-200]");
         return;
       };
 
       newAddress = parser.value_byte();
-
       if (!WITHIN(newAddress, 30, 200)) {
         SERIAL_PROTOCOLLNPGM("?New address out of range. [30-200]");
         return;
       }
-    } else if (!I2CPE_anyaxis) {
-      SERIAL_PROTOCOLLNPGM("?You must specify N or [XYZE].");
+    }
+    else if (!I2CPE_anyaxis) {
+      SERIAL_PROTOCOLLNPGM("?You must specify S or [XYZE].");
       return;
-    } else {
-      if (parser.seen('X')) newAddress = I2CPE_PRESET_ADDR_X;
+    }
+    else {
+           if (parser.seen('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;
@@ -970,12 +979,15 @@
     if (parse()) return;
 
     if (!I2CPE_addr) {
-      int8_t idx;
       LOOP_XYZE(i) {
-        if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0))
-          report_module_firmware(encoders[idx].get_address());
+        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);
+    }
+    else
+      report_module_firmware(I2CPE_addr);
   }
 
   /**
@@ -995,20 +1007,25 @@
   void I2CPositionEncodersMgr::M866() {
     if (parse()) return;
 
-    bool hasR = parser.seen('R');
+    const bool hasR = parser.seen('R');
 
-    if (I2CPE_idx < 0) {
-      int8_t idx;
+    if (I2CPE_idx == 0xFF) {
       LOOP_XYZE(i) {
-        if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) {
-          if (hasR) reset_error_count((uint8_t)idx, AxisEnum(i));
-          else report_error_count((uint8_t)idx, AxisEnum(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((uint8_t)I2CPE_idx, encoders[I2CPE_idx].get_axis());
-      else report_error_count((uint8_t)I2CPE_idx, encoders[I2CPE_idx].get_axis());
     }
+    else if (hasR)
+      reset_error_count(I2CPE_idx, encoders[I2CPE_idx].get_axis());
+    else
+      report_error_count(I2CPE_idx, encoders[I2CPE_idx].get_axis());
   }
 
   /**
@@ -1028,19 +1045,22 @@
   void I2CPositionEncodersMgr::M867() {
     if (parse()) return;
 
-    int8_t onoff = parser.seenval('S') ? parser.value_int() : -1;
+    const int8_t onoff = parser.seenval('S') ? parser.value_int() : -1;
 
-    if (I2CPE_idx < 0) {
-      int8_t idx;
+    if (I2CPE_idx == 0xFF) {
       LOOP_XYZE(i) {
-        if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) {
-          if (onoff == -1) enable_ec((uint8_t)idx, !encoders[idx].get_ec_enabled(), AxisEnum(i));
-          else enable_ec((uint8_t)idx, (bool)onoff, AxisEnum(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 {
-      if (onoff == -1) enable_ec((uint8_t)I2CPE_idx, !encoders[I2CPE_idx].get_ec_enabled(), encoders[I2CPE_idx].get_axis());
-      else enable_ec((uint8_t)I2CPE_idx, (bool)onoff, encoders[I2CPE_idx].get_axis());
+    }
+    else {
+      const bool ena = onoff == -1 ? !encoders[I2CPE_idx].get_ec_enabled() : !!onoff;
+      enable_ec(I2CPE_idx, ena, encoders[I2CPE_idx].get_axis());
     }
   }
 
@@ -1061,20 +1081,25 @@
   void I2CPositionEncodersMgr::M868() {
     if (parse()) return;
 
-    float newThreshold = parser.seenval('T') ? parser.value_float() : -9999;
+    const float newThreshold = parser.seenval('T') ? parser.value_float() : -9999;
 
-    if (I2CPE_idx < 0) {
-      int8_t idx;
+    if (I2CPE_idx == 0xFF) {
       LOOP_XYZE(i) {
-        if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) {
-          if (newThreshold != -9999) set_ec_threshold((uint8_t)idx, newThreshold, encoders[idx].get_axis());
-          else get_ec_threshold((uint8_t)idx, encoders[idx].get_axis());
+        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((uint8_t)I2CPE_idx, newThreshold, encoders[I2CPE_idx].get_axis());
-      else get_ec_threshold((uint8_t)I2CPE_idx, encoders[I2CPE_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());
   }
 
   /**
@@ -1092,13 +1117,16 @@
   void I2CPositionEncodersMgr::M869() {
     if (parse()) return;
 
-    if (I2CPE_idx < 0) {
-      int8_t idx;
+    if (I2CPE_idx == 0xFF) {
       LOOP_XYZE(i) {
-        if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0))
-          report_error((uint8_t)idx);
+        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((uint8_t)I2CPE_idx);
+    }
+    else
+      report_error(I2CPE_idx);
   }
 
-#endif
+#endif // I2C_POSITION_ENCODERS
diff --git a/Marlin/I2CPositionEncoder.h b/Marlin/I2CPositionEncoder.h
index fe0be390a5..a582a87b6d 100644
--- a/Marlin/I2CPositionEncoder.h
+++ b/Marlin/I2CPositionEncoder.h
@@ -95,195 +95,202 @@
   #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;
+  #define CHECK_IDX() do{ if (!WITHIN(idx, 0, I2CPE_ENCODER_CNT - 1)) return; }while(0)
 
   extern const char axis_codes[XYZE];
 
   typedef union {
-    volatile long   val = 0;
-    uint8_t         bval[4];
+    volatile int32_t val = 0;
+    uint8_t          bval[4];
   } i2cLong;
 
   class I2CPositionEncoder {
   private:
-    AxisEnum        encoderAxis             = I2CPE_DEF_AXIS;
+    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
+    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;
+    int       encoderTicksPerUnit = I2CPE_DEF_ENC_TICKS_UNIT,
+              stepperTicks        = I2CPE_DEF_TICKS_REV,
+              errorCount          = 0,
+              errorPrev           = 0;
 
-    float           ecThreshold             = I2CPE_DEF_EC_THRESH;
+    float     ecThreshold         = I2CPE_DEF_EC_THRESH;
 
-    bool            homed                   = false,
-                    trusted                 = false,
-                    initialised             = false,
-                    active                  = false,
-                    invert                  = false,
-                    ec                      = true;
+    bool      homed               = false,
+              trusted             = false,
+              initialised         = false,
+              active              = false,
+              invert              = false,
+              ec                  = true;
 
-    int             errorCount              = 0,
-                    errorPrev               = 0;
+    float     axisOffset          = 0;
 
-    float           axisOffset              = 0;
+    int32_t   axisOffsetTicks     = 0,
+              zeroOffset          = 0,
+              lastPosition        = 0,
+              position;
 
-    long            axisOffsetTicks         = 0,
-                    zeroOffset              = 0,
-                    lastPosition            = 0,
-                    position;
-
-    unsigned long   lastPositionTime        = 0,
-                    nextErrorCountTime      = 0,
-                    lastErrorTime;
+    millis_t  lastPositionTime    = 0,
+              nextErrorCountTime  = 0,
+              lastErrorTime;
 
     //double        positionMm; //calculate
 
     #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE)
-      uint8_t       errIdx = 0;
-      int           err[I2CPE_ERR_ARRAY_SIZE] = {0};
+      uint8_t errIdx = 0;
+      int     err[I2CPE_ERR_ARRAY_SIZE] = { 0 };
     #endif
 
+    //float        positionMm; //calculate
+
   public:
-    void init(uint8_t address, AxisEnum axis);
+    void init(const uint8_t address, const AxisEnum axis);
     void reset();
 
     void update();
 
     void set_homed();
 
-    long get_raw_count();
+    int32_t 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 float mm_from_count(const int32_t count) {
+      switch (type) {
+        default: return -1;
+        case I2CPE_ENC_TYPE_LINEAR:
+          return count / encoderTicksPerUnit;
+        case I2CPE_ENC_TYPE_ROTARY:
+          return (count * stepperTicks) / (encoderTicksPerUnit * planner.axis_steps_per_mm[encoderAxis]);
+      }
     }
 
-    FORCE_INLINE double get_position_mm() { return mm_from_count(get_position()); }
-    FORCE_INLINE long get_position() { return get_raw_count() - zeroOffset - axisOffsetTicks; }
+    FORCE_INLINE float get_position_mm() { return mm_from_count(get_position()); }
+    FORCE_INLINE int32_t get_position() { return get_raw_count() - zeroOffset - axisOffsetTicks; }
 
-    long get_axis_error_steps(bool report);
-    double get_axis_error_mm(bool report);
+    int32_t get_axis_error_steps(const bool report);
+    float get_axis_error_mm(const bool report);
 
-    void calibrate_steps_mm(int iter);
+    void calibrate_steps_mm(const uint8_t iter);
 
-    bool passes_test(bool report);
+    bool passes_test(const 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 void set_error_count(const int newCount) { errorCount = newCount; }
 
     FORCE_INLINE uint8_t get_address() { return i2cAddress; }
-    FORCE_INLINE void set_address(uint8_t addr) { i2cAddress = addr; }
+    FORCE_INLINE void set_address(const 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_active(const bool a) { active = a; }
 
-    FORCE_INLINE void set_inverted(bool i) { invert = i; }
+    FORCE_INLINE void set_inverted(const 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 void set_ec_enabled(const 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 void set_ec_method(const byte method) { ecMethod = method; }
 
     FORCE_INLINE float get_ec_threshold() { return ecThreshold; }
-    FORCE_INLINE void set_ec_threshold(float newThreshold) { ecThreshold = newThreshold; }
+    FORCE_INLINE void set_ec_threshold(const 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;
+      switch (type) {
+        default: return 0;
+        case I2CPE_ENC_TYPE_LINEAR:
+          return encoderTicksPerUnit;
+        case I2CPE_ENC_TYPE_ROTARY:
+          return (int)((encoderTicksPerUnit / stepperTicks) * planner.axis_steps_per_mm[encoderAxis]);
+      }
     }
 
     FORCE_INLINE int get_ticks_unit() { return encoderTicksPerUnit; }
-    FORCE_INLINE void set_ticks_unit(int ticks) { encoderTicksPerUnit = ticks; }
+    FORCE_INLINE void set_ticks_unit(const int ticks) { encoderTicksPerUnit = ticks; }
 
     FORCE_INLINE uint8_t get_type() { return type; }
-    FORCE_INLINE void set_type(byte newType) { type = newType; }
+    FORCE_INLINE void set_type(const byte newType) { type = newType; }
 
     FORCE_INLINE int get_stepper_ticks() { return stepperTicks; }
-    FORCE_INLINE void set_stepper_ticks(int ticks) { stepperTicks = ticks; }
+    FORCE_INLINE void set_stepper_ticks(const int ticks) { stepperTicks = ticks; }
 
     FORCE_INLINE float get_axis_offset() { return axisOffset; }
-    FORCE_INLINE void set_axis_offset(float newOffset) {
+    FORCE_INLINE void set_axis_offset(const float newOffset) {
       axisOffset = newOffset;
-      axisOffsetTicks = (long)(axisOffset * get_encoder_ticks_mm());
+      axisOffsetTicks = int32_t(axisOffset * get_encoder_ticks_mm());
     }
 
-    FORCE_INLINE void set_current_position(float newPositionMm) {
+    FORCE_INLINE void set_current_position(const float newPositionMm) {
       set_axis_offset(get_position_mm() - newPositionMm + axisOffset);
     }
   };
 
   class I2CPositionEncodersMgr {
   private:
-    bool I2CPE_anyaxis;
-    uint8_t I2CPE_addr;
-    int8_t I2CPE_idx;
+    static bool I2CPE_anyaxis;
+    static uint8_t I2CPE_addr, I2CPE_idx;
 
   public:
-    void init(void);
+
+    static 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(); }
+    static void update(void) { LOOP_PE(i) encoders[i].update(); }
 
-    void homed(AxisEnum axis) {
+    static void homed(const 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);
+    static void report_position(const int8_t idx, const bool units, const bool noOffset);
 
-    void report_status(uint8_t idx) {
-      CHECK_IDX
+    static void report_status(const int8_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
+    static void report_error(const int8_t idx) {
+      CHECK_IDX();
       encoders[idx].get_axis_error_steps(true);
     }
 
-    void test_axis(uint8_t idx) {
-      CHECK_IDX
+    static void test_axis(const int8_t idx) {
+      CHECK_IDX();
       encoders[idx].test_axis();
     }
 
-    void calibrate_steps_mm(uint8_t idx, int iterations) {
-      CHECK_IDX
+    static void calibrate_steps_mm(const int8_t idx, const 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);
+    static void change_module_address(const uint8_t oldaddr, const uint8_t newaddr);
+    static void report_module_firmware(const uint8_t address);
 
-    void report_error_count(uint8_t idx, AxisEnum axis) {
-      CHECK_IDX
+    static void report_error_count(const int8_t idx, const 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
+    static void reset_error_count(const int8_t idx, const 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
+    static void enable_ec(const int8_t idx, const bool enabled, const AxisEnum axis) {
+      CHECK_IDX();
       encoders[idx].set_ec_enabled(enabled);
       SERIAL_ECHOPAIR("Error correction on ", axis_codes[axis]);
       SERIAL_ECHOPGM(" axis is ");
@@ -291,66 +298,62 @@
       SERIAL_ECHOLNPGM("abled.");
     }
 
-    void set_ec_threshold(uint8_t idx, float newThreshold, AxisEnum axis) {
-      CHECK_IDX
+    static void set_ec_threshold(const int8_t idx, const float newThreshold, const 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();
+    static void get_ec_threshold(const int8_t idx, const AxisEnum axis) {
+      CHECK_IDX();
+      const 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) {
+    static int8_t idx_from_axis(const AxisEnum axis) {
       LOOP_PE(i)
         if (encoders[i].get_axis() == axis) return i;
-
       return -1;
     }
 
-    int8_t idx_from_addr(uint8_t addr) {
+    static int8_t idx_from_addr(const uint8_t addr) {
       LOOP_PE(i)
         if (encoders[i].get_address() == addr) return i;
-
       return -1;
     }
 
-    int8_t parse();
+    static int8_t parse();
 
-    void M860();
-    void M861();
-    void M862();
-    void M863();
-    void M864();
-    void M865();
-    void M866();
-    void M867();
-    void M868();
-    void M869();
+    static void M860();
+    static void M861();
+    static void M862();
+    static void M863();
+    static void M864();
+    static void M865();
+    static void M866();
+    static void M867();
+    static void M868();
+    static void M869();
 
-    I2CPositionEncoder encoders[I2CPE_ENCODER_CNT];
+    static 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(); }
+  FORCE_INLINE static void gcode_M860() { I2CPEM.M860(); }
+  FORCE_INLINE static void gcode_M861() { I2CPEM.M861(); }
+  FORCE_INLINE static void gcode_M862() { I2CPEM.M862(); }
+  FORCE_INLINE static void gcode_M863() { I2CPEM.M863(); }
+  FORCE_INLINE static void gcode_M864() { I2CPEM.M864(); }
+  FORCE_INLINE static void gcode_M865() { I2CPEM.M865(); }
+  FORCE_INLINE static void gcode_M866() { I2CPEM.M866(); }
+  FORCE_INLINE static void gcode_M867() { I2CPEM.M867(); }
+  FORCE_INLINE static void gcode_M868() { I2CPEM.M868(); }
+  FORCE_INLINE static void gcode_M869() { I2CPEM.M869(); }
 
 #endif //I2C_POSITION_ENCODERS
 #endif //I2CPOSENC_H
-
-
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 208abc5eb3..b6dd4c2c8c 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -5141,7 +5141,7 @@ void home_all_axes() { gcode_G28(true); }
      *   T   Don't calibrate tower angle corrections
      *
      *   Cn.nn Calibration precision; when omitted calibrates to maximum precision
-     *   
+     *
      *   Fn  Force to run at least n iterations and takes the best result
      *
      *   Vn  Verbose level:
@@ -5687,7 +5687,7 @@ inline void gcode_G92() {
             update_software_endstops((AxisEnum)i);
 
             #if ENABLED(I2C_POSITION_ENCODERS)
-              I2CPEM.encoders[I2CPEM.idx_from_axis((AxisEnum) i)].set_axis_offset(position_shift[i]);
+              I2CPEM.encoders[I2CPEM.idx_from_axis((AxisEnum)i)].set_axis_offset(position_shift[i]);
             #endif
 
           #endif
diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index d2bcda6580..73d0fc57b2 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -276,9 +276,7 @@
 #if ENABLED(I2C_POSITION_ENCODERS)
   #if DISABLED(BABYSTEPPING)
     #error "I2C_POSITION_ENCODERS requires BABYSTEPPING."
-  #endif
-
-  #if I2CPE_ENCODER_CNT > 5 || I2CPE_ENCODER_CNT < 1
+  #elif !WITHIN(I2CPE_ENCODER_CNT, 1, 5)
     #error "I2CPE_ENCODER_CNT must be between 1 and 5."
   #endif
 #endif
diff --git a/Marlin/ubl_G29.cpp b/Marlin/ubl_G29.cpp
index 294e3616a3..f77f3746a0 100644
--- a/Marlin/ubl_G29.cpp
+++ b/Marlin/ubl_G29.cpp
@@ -328,7 +328,8 @@
       g29_repetition_cnt = parser.has_value() ? parser.value_int() : 1;
       if (g29_repetition_cnt >= GRID_MAX_POINTS) {
         set_all_mesh_points_to_value(NAN);
-      } else {
+      }
+      else {
         while (g29_repetition_cnt--) {
           if (cnt > 20) { cnt = 0; idle(); }
           const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
@@ -1454,7 +1455,7 @@
     void unified_bed_leveling::fine_tune_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map) {
       if (!parser.seen('R'))    // fine_tune_mesh() is special. If no repetition count flag is specified
         g29_repetition_cnt = 1;   // do exactly one mesh location. Otherwise use what the parser decided.
-      
+
       #if ENABLED(UBL_MESH_EDIT_MOVES_Z)
         const bool is_offset = parser.seen('H');
         const float h_offset = is_offset ? parser.value_linear_units() : Z_CLEARANCE_BETWEEN_PROBES;
@@ -1463,7 +1464,7 @@
           return;
         }
       #endif
-      
+
       mesh_index_pair location;
 
       if (!position_is_reachable_xy(lx, ly)) {