From e82a345e194b0132e5b5f3c7bfd1bbf31abb57f4 Mon Sep 17 00:00:00 2001
From: Panagiotis Papadakos <papadako@ics.forth.gr>
Date: Thu, 30 Oct 2025 17:50:48 +0200
Subject: [PATCH 01/12] (servo): speed up DcServoDriver::toAnalogRange with
 cached scaling and float math; should be a lot faster and maybe with less
 jitter
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- Cache pwm min/max and gain and recompute only when pwmMinimum, pwmMaximum, velocityMax, or analogWriteRange change
- Replace double paths with single-precision ops (lround→lroundf, fabs→fabsf, use fmaf) -> double precision not crucial and not supported by FPUs
- Remove per-tick map() and % → counts work
- pwmUpdate fabs-> labs
- Generally, shorter and more consistent control-tick critical section. Might reduce preemption impact and free ISR headroom
---
 src/lib/axis/motor/servo/dc/DcServoDriver.cpp |  5 +-
 src/lib/axis/motor/servo/dc/DcServoDriver.h   | 83 +++++++++++++++----
 2 files changed, 71 insertions(+), 17 deletions(-)

diff --git a/src/lib/axis/motor/servo/dc/DcServoDriver.cpp b/src/lib/axis/motor/servo/dc/DcServoDriver.cpp
index f2eb23978..2f8055dd4 100644
--- a/src/lib/axis/motor/servo/dc/DcServoDriver.cpp
+++ b/src/lib/axis/motor/servo/dc/DcServoDriver.cpp
@@ -10,15 +10,16 @@
 ServoDcDriver::ServoDcDriver(uint8_t axisNumber, const ServoPins *Pins, const ServoSettings *Settings, float pwmMinimum, float pwmMaximum)
                             :ServoDriver(axisNumber, Pins, Settings) {
   if (axisNumber < 1 || axisNumber > 9) return;
-
   this->pwmMinimum.valueDefault = pwmMinimum;
   this->pwmMaximum.valueDefault = pwmMaximum;
+  // initialize caches up front
+  recomputeScalingIfNeeded();
 }
 
 float ServoDcDriver::setMotorVelocity(float velocity)  {
   velocity = ServoDriver::setMotorVelocity(velocity);
 
-  pwmUpdate(fabs(toAnalogRange(velocity)));
+  pwmUpdate(labs(toAnalogRange(velocity)));
 
   return velocity;
 }
diff --git a/src/lib/axis/motor/servo/dc/DcServoDriver.h b/src/lib/axis/motor/servo/dc/DcServoDriver.h
index 450de6cfb..2cddf75b0 100644
--- a/src/lib/axis/motor/servo/dc/DcServoDriver.h
+++ b/src/lib/axis/motor/servo/dc/DcServoDriver.h
@@ -3,6 +3,7 @@
 #pragma once
 
 #include <Arduino.h>
+#include <math.h>            // fabsf, lroundf, fmaf
 #include "../../../../../Common.h"
 
 #if defined(SERVO_PE_PRESENT) || defined(SERVO_EE_PRESENT) || defined(SERVO_TMC2130_DC_PRESENT) || defined(SERVO_TMC5160_DC_PRESENT)
@@ -34,23 +35,36 @@ class ServoDcDriver : public ServoDriver {
 
   protected:
     // convert from encoder counts per second to analogWriteRange units to roughly match velocity
+    // we expect a linear scaling for the motors (maybe in the future we can be smarter?)
+    // uses cached min/max counts and a precomputed gain
+    // only float ops since usually there is no double FPU
     long toAnalogRange(float velocity) {
-      long sign = 1;
-      if (velocity < 0.0F) {
-        velocity = -velocity;
-        sign = -1;
-      }
-  
-      long power = 0;
-      if (velocity != 0.0F) {
-        power = lround(((float)velocity/velocityMax)*(analogWriteRange - 1));
-        long pwmMin = lround(pwmMinimum.value/100.0F*(analogWriteRange - 1));
-        long pwmMax = lround(pwmMaximum.value/100.0F*(analogWriteRange - 1));
-
-        power = map(power, 0, analogWriteRange - 1, pwmMin, pwmMax);
+      // Update caches only if inputs changed
+      recomputeScalingIfNeeded();
+
+      // Extract sign and work with magnitude
+      int sign = 1;
+      if (velocity < 0.0F) { velocity = -velocity; sign = -1; }
+
+      if (velocity == 0.0F || velocityMaxCached <= 0.0f) {
+        return 0; // early outvelocityMaxCached
       }
 
-      return power*sign;
+      // Clamp to velocityMax to avoid over-range math.
+      float vAbs = velocity;
+      if (vAbs > velocityMaxCached) vAbs = velocityMaxCached;
+
+      // Linear map: counts = countsMin + vAbs * gain
+      // fmaf keeps one rounding and is very fast on some FPUS(M7).
+      // Currenty we always set the power to overcome static friction to the min counts
+      // even for near zero velocity where the PID jitters around zero
+      // TODO! Consider deadband handling here
+      float countsF = fmaf(vAbs, velToCountsGain, (float)countsMinCached);
+
+      // Single float→int rounding at the end
+      long power = (long)lroundf(countsF);
+
+      return power * sign;
     }
 
     // motor control update
@@ -58,12 +72,51 @@ class ServoDcDriver : public ServoDriver {
 
     long analogWriteRange = SERVO_ANALOG_WRITE_RANGE;
 
-    // runtime adjustable settings
+    // runtime adjustable settings (percent 0..100)
     AxisParameter pwmMinimum = {NAN, NAN, NAN, 0.0, 100.0, AXP_FLOAT_IMMEDIATE, AXPN_MIN_PWR};
     AxisParameter pwmMaximum = {NAN, NAN, NAN, 0.0, 100.0, AXP_FLOAT_IMMEDIATE, AXPN_MAX_PWR};
 
     const int numParameters = 3;
     AxisParameter* parameter[4] = {&invalid, &acceleration, &pwmMinimum, &pwmMaximum};
+
+  private:
+    // Cached scaling (recomputed only when inputs change)
+    // Detect changes to pwmMinimum.value, pwmMaximum.value, velocityMax, analogWriteRange.
+    float   pwmMinPctCached   = -1.0f;
+    float   pwmMaxPctCached   = -1.0f;
+    float   velocityMaxCached = 0.0f;
+    long    analogMaxCached   = -1;
+
+    int32_t countsMinCached   = 0;      // integer min duty in counts
+    int32_t countsMaxCached   = 0;      // integer max duty in counts
+    float   velToCountsGain   = 0.0f;   // counts per (encoder count/s)
+
+    // Recompute cache if any dependency changed.
+    inline void recomputeScalingIfNeeded() {
+      const long analogMaxNow = (analogWriteRange - 1);
+
+      if (pwmMinPctCached   != pwmMinimum.value ||
+          pwmMaxPctCached   != pwmMaximum.value ||
+          velocityMaxCached != velocityMax ||
+          analogMaxCached   != analogMaxNow)
+      {
+        pwmMinPctCached   = pwmMinimum.value;   // percent 0..100
+        pwmMaxPctCached   = pwmMaximum.value;   // percent 0..100
+        velocityMaxCached = velocityMax;
+        analogMaxCached   = analogMaxNow;
+
+        // Convert % to float counts once, then round once.
+        const float minCountsF = (pwmMinPctCached * 0.01f) * (float)analogMaxCached;
+        const float maxCountsF = (pwmMaxPctCached * 0.01f) * (float)analogMaxCached;
+
+        countsMinCached = (int32_t)lroundf(minCountsF);
+        countsMaxCached = (int32_t)lroundf(maxCountsF);
+        if (countsMaxCached < countsMinCached) countsMaxCached = countsMinCached; // safety
+
+        const int span = (int)(countsMaxCached - countsMinCached);
+        velToCountsGain = (velocityMaxCached > 0.0f) ? ((float)span / velocityMaxCached) : 0.0f;
+      }
+    }
 };
 
 #endif

From 2e74086d602c50a765e9b1736c4410fba0d76318 Mon Sep 17 00:00:00 2001
From: Panagiotis Papadakos <papadako@ics.forth.gr>
Date: Thu, 30 Oct 2025 18:22:47 +0200
Subject: [PATCH 02/12] servo DC: optional zero-crossing hysteresis with
 immediate zero exit

Add SERVO_HYSTERESIS_ENABLE + SERVO_HYST_ENTER_CPS/SERVO_HYST_EXIT_CPS to prevent PWM chatter near 0 (preserve existing linear mapping)
---
 src/lib/axis/motor/servo/dc/DcServoDriver.h | 81 +++++++++++++++------
 1 file changed, 59 insertions(+), 22 deletions(-)

diff --git a/src/lib/axis/motor/servo/dc/DcServoDriver.h b/src/lib/axis/motor/servo/dc/DcServoDriver.h
index 2cddf75b0..7f084c854 100644
--- a/src/lib/axis/motor/servo/dc/DcServoDriver.h
+++ b/src/lib/axis/motor/servo/dc/DcServoDriver.h
@@ -15,6 +15,18 @@
   #define SERVO_ANALOG_WRITE_RANGE ANALOG_WRITE_RANGE
 #endif
 
+// Enable to apply hysteresis around zero velocity
+#define SERVO_HYSTERESIS_ENABLE
+
+// Thresholds in encoder counts/sec
+#ifndef SERVO_HYST_ENTER_CPS
+  #define SERVO_HYST_ENTER_CPS 1.2f   // must exceed this to LEAVE zero
+#endif
+#ifndef SERVO_HYST_EXIT_CPS
+  #define SERVO_HYST_EXIT_CPS 0.6f    // drop below this to RETURN to zero
+#endif
+
+
 #include "../ServoDriver.h"
 
 class ServoDcDriver : public ServoDriver {
@@ -38,34 +50,54 @@ class ServoDcDriver : public ServoDriver {
     // we expect a linear scaling for the motors (maybe in the future we can be smarter?)
     // uses cached min/max counts and a precomputed gain
     // only float ops since usually there is no double FPU
-    long toAnalogRange(float velocity) {
-      // Update caches only if inputs changed
-      recomputeScalingIfNeeded();
+  long toAnalogRange(float velocity) {
+    // Update caches only if inputs changed
+    recomputeScalingIfNeeded();
 
-      // Extract sign and work with magnitude
-      int sign = 1;
-      if (velocity < 0.0F) { velocity = -velocity; sign = -1; }
+    // Extract sign and work with magnitude
+    int sign = 1;
+    if (velocity < 0.0F) { velocity = -velocity; sign = -1; }
 
-      if (velocity == 0.0F || velocityMaxCached <= 0.0f) {
-        return 0; // early outvelocityMaxCached
-      }
+    if (velocity == 0.0F || velocityMaxCached <= 0.0f) {
 
-      // Clamp to velocityMax to avoid over-range math.
-      float vAbs = velocity;
-      if (vAbs > velocityMaxCached) vAbs = velocityMaxCached;
+      #ifdef SERVO_HYSTERESIS_ENABLE
+        zeroHoldSign = 0; // ensure immediate exit and clear latch on exact zero
+      #endif
 
-      // Linear map: counts = countsMin + vAbs * gain
-      // fmaf keeps one rounding and is very fast on some FPUS(M7).
-      // Currenty we always set the power to overcome static friction to the min counts
-      // even for near zero velocity where the PID jitters around zero
-      // TODO! Consider deadband handling here
-      float countsF = fmaf(vAbs, velToCountsGain, (float)countsMinCached);
+      return 0; // early out
+    }
 
-      // Single float→int rounding at the end
-      long power = (long)lroundf(countsF);
+    // Clamp to velocityMax to avoid over-range math.
+    float vAbs = velocity; // already absolute
+    if (vAbs > velocityMaxCached) vAbs = velocityMaxCached;
+
+    #ifdef SERVO_HYSTERESIS_ENABLE
+      // Hysteresis around zero: require a larger enter threshold to leave zero,
+      // and a smaller "exit" threshold to return to zero. This prevents chatter
+      // when the PID jitters around zero
+      if (zeroHoldSign == 0) {
+        // currently at zero: must exceed enter threshold to start moving
+        if (vAbs < SERVO_HYST_ENTER_CPS) return 0;
+        zeroHoldSign = (sign >= 0) ? 1 : -1; // latch direction
+      } else {
+        // currently moving: if we drop below exit threshold, snap back to zero
+        if (vAbs < SERVO_HYST_EXIT_CPS) { zeroHoldSign = 0; return 0; }
+        // allow direction change if command flips while above thresholds
+        if ((sign >= 0 ? 1 : -1) != zeroHoldSign) zeroHoldSign = (sign >= 0) ? 1 : -1;
+      }
+      // use latched direction while moving
+      sign = (zeroHoldSign < 0) ? -1 : 1;
+    #endif
 
-      return power * sign;
-    }
+    // Linear map: counts = countsMin + vAbs * gain
+    // fmaf keeps one rounding and is very fast on some FPUS(M7).
+    float countsF = fmaf(vAbs, velToCountsGain, (float)countsMinCached);
+
+    // Single float→int rounding at the end
+    long power = (long)lroundf(countsF);
+
+    return power * sign;
+  }
 
     // motor control update
     virtual void pwmUpdate(long power) { }
@@ -117,6 +149,11 @@ class ServoDcDriver : public ServoDriver {
         velToCountsGain = (velocityMaxCached > 0.0f) ? ((float)span / velocityMaxCached) : 0.0f;
       }
     }
+
+    #ifdef SERVO_HYSTERESIS_ENABLE
+      int8_t zeroHoldSign = 0; // 0: at zero; +1 / -1: direction while "moving"
+    #endif
+
 };
 
 #endif

From 643cec991baa07ea9273776c06b30847b7c6d10b Mon Sep 17 00:00:00 2001
From: Panagiotis Papadakos <papadako@ics.forth.gr>
Date: Thu, 30 Oct 2025 18:39:39 +0200
Subject: [PATCH 03/12] =?UTF-8?q?DC=20servo:=20add=20overflow-safe=20sigma?=
 =?UTF-8?q?=E2=80=93delta=20PWM=20dithering=20for=20sub-count=20resolution?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- Introduce SERVO_SIGMA_DELTA_DITHERING (residual, non-overflowing implementation)
- Dither float PWM counts to integer per tick so long-term average matches target for DC motors
- Improve effective torque resolution near sidereal
- Reduces bias and micro-jitter vs. trunc/round-only outputs
- Reset residual on zero-output/disable to avoid stale carryover
- Uses single-precision math
---
 src/lib/axis/motor/servo/ServoDriver.h        | 13 ++++-
 src/lib/axis/motor/servo/dc/DcServoDriver.h   | 24 +++++++--
 .../axis/motor/servo/dc/SigmaDeltaDither.h    | 52 +++++++++++++++++++
 3 files changed, 84 insertions(+), 5 deletions(-)
 create mode 100644 src/lib/axis/motor/servo/dc/SigmaDeltaDither.h

diff --git a/src/lib/axis/motor/servo/ServoDriver.h b/src/lib/axis/motor/servo/ServoDriver.h
index f58a67b22..61c09e5e5 100644
--- a/src/lib/axis/motor/servo/ServoDriver.h
+++ b/src/lib/axis/motor/servo/ServoDriver.h
@@ -10,6 +10,10 @@
 
 #ifdef SERVO_MOTOR_PRESENT
 
+#ifdef SERVO_SIGMA_DELTA_DITHERING
+  #include "dc/SigmaDeltaDither.h"
+#endif
+
 typedef struct ServoPins {
   int16_t ph1; // step
   int16_t ph1State;
@@ -71,7 +75,7 @@ class ServoDriver {
     // get status info.
     // this is a required method for the Axis class
     DriverStatus getStatus() { return status; }
-   
+
     // calibrate the motor if required
     virtual void calibrateDriver() {}
 
@@ -80,7 +84,7 @@ class ServoDriver {
 
   protected:
     virtual void readStatus() {}
-    
+
     int axisNumber;
     char axisPrefix[32]; // prefix for debug messages
 
@@ -115,6 +119,11 @@ class ServoDriver {
 
     const int numParameters = 2;
     AxisParameter* parameter[2] = {&invalid, &acceleration};
+
+    #ifdef SERVO_SIGMA_DELTA_DITHERING
+      SigmaDeltaDither sigmaDelta;  // carries fractional residue between ticks
+    #endif
+
 };
 
 #endif
diff --git a/src/lib/axis/motor/servo/dc/DcServoDriver.h b/src/lib/axis/motor/servo/dc/DcServoDriver.h
index 7f084c854..dff4ce49d 100644
--- a/src/lib/axis/motor/servo/dc/DcServoDriver.h
+++ b/src/lib/axis/motor/servo/dc/DcServoDriver.h
@@ -26,6 +26,9 @@
   #define SERVO_HYST_EXIT_CPS 0.6f    // drop below this to RETURN to zero
 #endif
 
+ifdef SERVO_SIGMA_DELTA_DITHERING
+  #include "SigmaDeltaDither.h"
+#endif
 
 #include "../ServoDriver.h"
 
@@ -64,6 +67,10 @@ class ServoDcDriver : public ServoDriver {
         zeroHoldSign = 0; // ensure immediate exit and clear latch on exact zero
       #endif
 
+      #ifdef SERVO_SIGMA_DELTA_DITHERING
+        sigmaDelta.reset(); // reset dithering residue when output is zero
+      #endif
+
       return 0; // early out
     }
 
@@ -81,7 +88,13 @@ class ServoDcDriver : public ServoDriver {
         zeroHoldSign = (sign >= 0) ? 1 : -1; // latch direction
       } else {
         // currently moving: if we drop below exit threshold, snap back to zero
-        if (vAbs < SERVO_HYST_EXIT_CPS) { zeroHoldSign = 0; return 0; }
+        if (vAbs < SERVO_HYST_EXIT_CPS) {
+          zeroHoldSign = 0;
+          #ifdef SERVO_SIGMA_DELTA_DITHERING
+            sigmaDelta.reset(); // also reset when snapping back to zero
+          #endif
+          return 0;
+        }
         // allow direction change if command flips while above thresholds
         if ((sign >= 0 ? 1 : -1) != zeroHoldSign) zeroHoldSign = (sign >= 0) ? 1 : -1;
       }
@@ -93,8 +106,13 @@ class ServoDcDriver : public ServoDriver {
     // fmaf keeps one rounding and is very fast on some FPUS(M7).
     float countsF = fmaf(vAbs, velToCountsGain, (float)countsMinCached);
 
-    // Single float→int rounding at the end
-    long power = (long)lroundf(countsF);
+    #ifdef SERVO_SIGMA_DELTA_DITHERING
+      // Dither the floating counts to an integer so the time-average equals countsF
+      int32_t power = sigmaDelta.dither_counts(countsF, countsMinCached, countsMaxCached);
+    #else
+      // Single float→int rounding at the end
+      long power = (long)lroundf(countsF);
+    #endif
 
     return power * sign;
   }
diff --git a/src/lib/axis/motor/servo/dc/SigmaDeltaDither.h b/src/lib/axis/motor/servo/dc/SigmaDeltaDither.h
new file mode 100644
index 000000000..127306746
--- /dev/null
+++ b/src/lib/axis/motor/servo/dc/SigmaDeltaDither.h
@@ -0,0 +1,52 @@
+// -----------------------------------------------------------------------------
+// Overflow-safe Sigma–Delta PWM Dithering
+// Enable with:   #define SERVO_SIGMA_DELTA_DITHERING ON
+//
+// Purpose:
+//   When PWM resolution is low (e.g., sidereal ≈ a few counts), this modulator
+//   time-averages between adjacent integer counts so the long-term average
+//   equals a floating target (e.g., 4.4 → mix of 4 and 5). This code never
+//   overflows because it only carries the fractional residual in [0,1).
+// -----------------------------------------------------------------------------
+
+#ifdef SERVO_SIGMA_DELTA_DITHERING
+
+// Lightweight state holder for first-order sigma–delta dithering
+// We store only the fractional residual between ticks
+struct SigmaDeltaDither {
+    // Fractional residue in [0,1). Carries the part we couldn't emit this tick
+    float resid = 0.0f;
+
+    // Reset internal state (call when you reinitialize control or modes change)
+    inline void reset() { resid = 0.0f; }
+
+    // convert a floating desired count into an integer count for this tick,
+    // such that the time-average of outputs equals the floating target
+    inline int32_t dither_counts(float desiredCounts,
+                                    int32_t minCount,
+                                    int32_t maxCount)
+    {
+        // Safety clamp the desired average to valid bounds.
+        if (desiredCounts < (float)minCount) desiredCounts = (float)minCount;
+        if (desiredCounts > (float)maxCount) desiredCounts = (float)maxCount;
+
+        // Add residual: carry fractional error forward from previous tick
+        // e.g. 4.4 + 0.7 = 5.1 → emit 5 now, resid becomes 0.1
+        float sum = desiredCounts + resid;
+
+        // Emit the integer part of this tick
+        int32_t out = (int32_t)floorf(sum);
+
+        // Keep the fractional residue for next tick (always in [0,1))
+        resid = sum - (float)out;
+
+        // safety clamp (probably not needed, but just in case)
+        if (out < minCount) out = minCount;
+        if (out > maxCount) out = maxCount;
+
+        return out; // Integer PWM counts to write this tick
+    }
+
+};
+
+#endif

From 0cfff7815e3f67b202ba2fb480e66a065c31963d Mon Sep 17 00:00:00 2001
From: Panagiotis Papadakos <papadako@gmail.com>
Date: Thu, 30 Oct 2025 19:37:14 +0200
Subject: [PATCH 04/12] servo hysterisis: typo defined hysterisis things only
 if #ifdef SERVO_HYSTERESIS_ENABLE

---
 src/lib/axis/motor/servo/dc/DcServoDriver.h | 24 ++++++++++-----------
 1 file changed, 12 insertions(+), 12 deletions(-)

diff --git a/src/lib/axis/motor/servo/dc/DcServoDriver.h b/src/lib/axis/motor/servo/dc/DcServoDriver.h
index dff4ce49d..bd0ec5700 100644
--- a/src/lib/axis/motor/servo/dc/DcServoDriver.h
+++ b/src/lib/axis/motor/servo/dc/DcServoDriver.h
@@ -16,18 +16,18 @@
 #endif
 
 // Enable to apply hysteresis around zero velocity
-#define SERVO_HYSTERESIS_ENABLE
-
-// Thresholds in encoder counts/sec
-#ifndef SERVO_HYST_ENTER_CPS
-  #define SERVO_HYST_ENTER_CPS 1.2f   // must exceed this to LEAVE zero
-#endif
-#ifndef SERVO_HYST_EXIT_CPS
-  #define SERVO_HYST_EXIT_CPS 0.6f    // drop below this to RETURN to zero
-#endif
-
-ifdef SERVO_SIGMA_DELTA_DITHERING
-  #include "SigmaDeltaDither.h"
+#ifdef SERVO_HYSTERESIS_ENABLE
+  // Thresholds in encoder counts/sec
+  #ifndef SERVO_HYST_ENTER_CPS
+    #define SERVO_HYST_ENTER_CPS 1.2f   // must exceed this to LEAVE zero
+  #endif
+  #ifndef SERVO_HYST_EXIT_CPS
+    #define SERVO_HYST_EXIT_CPS 0.6f    // drop below this to RETURN to zero
+  #endif
+
+  ifdef SERVO_SIGMA_DELTA_DITHERING
+    #include "SigmaDeltaDither.h"
+  #endif
 #endif
 
 #include "../ServoDriver.h"

From ef76359bbac2c32d73e8cdd9a4139ab93253109f Mon Sep 17 00:00:00 2001
From: Panagiotis Papadakos <papadako@gmail.com>
Date: Thu, 30 Oct 2025 20:07:18 +0200
Subject: [PATCH 05/12] typos

---
 src/lib/axis/motor/servo/dc/DcServoDriver.h    | 2 +-
 src/lib/axis/motor/servo/dc/SigmaDeltaDither.h | 2 ++
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/lib/axis/motor/servo/dc/DcServoDriver.h b/src/lib/axis/motor/servo/dc/DcServoDriver.h
index bd0ec5700..ee71aaf81 100644
--- a/src/lib/axis/motor/servo/dc/DcServoDriver.h
+++ b/src/lib/axis/motor/servo/dc/DcServoDriver.h
@@ -25,7 +25,7 @@
     #define SERVO_HYST_EXIT_CPS 0.6f    // drop below this to RETURN to zero
   #endif
 
-  ifdef SERVO_SIGMA_DELTA_DITHERING
+  #ifdef SERVO_SIGMA_DELTA_DITHERING
     #include "SigmaDeltaDither.h"
   #endif
 #endif
diff --git a/src/lib/axis/motor/servo/dc/SigmaDeltaDither.h b/src/lib/axis/motor/servo/dc/SigmaDeltaDither.h
index 127306746..3d250cddd 100644
--- a/src/lib/axis/motor/servo/dc/SigmaDeltaDither.h
+++ b/src/lib/axis/motor/servo/dc/SigmaDeltaDither.h
@@ -9,6 +9,8 @@
 //   overflows because it only carries the fractional residual in [0,1).
 // -----------------------------------------------------------------------------
 
+#pragma once
+
 #ifdef SERVO_SIGMA_DELTA_DITHERING
 
 // Lightweight state holder for first-order sigma–delta dithering

From bf992d9cd29cafa3976f27c480020bee73f78db5 Mon Sep 17 00:00:00 2001
From: Panagiotis Papadakos <papadako@gmail.com>
Date: Fri, 31 Oct 2025 00:50:48 +0200
Subject: [PATCH 06/12] debugging messages

---
 src/lib/axis/motor/servo/dc/DcServoDriver.h | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/src/lib/axis/motor/servo/dc/DcServoDriver.h b/src/lib/axis/motor/servo/dc/DcServoDriver.h
index ee71aaf81..0722a8ba7 100644
--- a/src/lib/axis/motor/servo/dc/DcServoDriver.h
+++ b/src/lib/axis/motor/servo/dc/DcServoDriver.h
@@ -19,10 +19,10 @@
 #ifdef SERVO_HYSTERESIS_ENABLE
   // Thresholds in encoder counts/sec
   #ifndef SERVO_HYST_ENTER_CPS
-    #define SERVO_HYST_ENTER_CPS 1.2f   // must exceed this to LEAVE zero
+    #define SERVO_HYST_ENTER_CPS 20.0f   // must exceed this to LEAVE zero (1/10 of sidereal speed of 92 counts / sec)
   #endif
   #ifndef SERVO_HYST_EXIT_CPS
-    #define SERVO_HYST_EXIT_CPS 0.6f    // drop below this to RETURN to zero
+    #define SERVO_HYST_EXIT_CPS 10.0f    // drop below this to RETURN to zero (half of the above)
   #endif
 
   #ifdef SERVO_SIGMA_DELTA_DITHERING
@@ -155,6 +155,11 @@ class ServoDcDriver : public ServoDriver {
         velocityMaxCached = velocityMax;
         analogMaxCached   = analogMaxNow;
 
+        VF("MSG:"); V(axisPrefix); VF("pwmMin="); V(pwmMinPctCached); VLF(" %");
+        VF("MSG:"); V(axisPrefix); VF("pwmMax="); V(pwmMaxPctCached); VLF(" %");
+        VF("MSG:"); V(axisPrefix); VF("Vmax="); V(velocityMaxCached); VLF(" steps/s");
+        VF("MSG:"); V(axisPrefix); VF("pwm units="); V(analogMaxCached); VLF(" pwm Units");
+
         // Convert % to float counts once, then round once.
         const float minCountsF = (pwmMinPctCached * 0.01f) * (float)analogMaxCached;
         const float maxCountsF = (pwmMaxPctCached * 0.01f) * (float)analogMaxCached;
@@ -165,6 +170,7 @@ class ServoDcDriver : public ServoDriver {
 
         const int span = (int)(countsMaxCached - countsMinCached);
         velToCountsGain = (velocityMaxCached > 0.0f) ? ((float)span / velocityMaxCached) : 0.0f;
+        VF("MSG:"); V(axisPrefix); VF("velToCountsGain="); V(velToCountsGain); VLF(" from velocity -> pwm units");
       }
     }
 

From 9755e2a649620484d1dee369baa7e3bc4f56e9ab Mon Sep 17 00:00:00 2001
From: Panagiotis Papadakos <papadako@gmail.com>
Date: Fri, 31 Oct 2025 01:08:19 +0200
Subject: [PATCH 07/12] servo DC toAnalogRange: previously for velocity = 0 we
 were offseting to pwmMin. Change it so that we map power to 0-pwmMax. If less
 than pwmMin then set it to pwmMin.

---
 src/lib/axis/motor/servo/dc/DcServoDriver.h | 23 ++++++++++++++-------
 1 file changed, 15 insertions(+), 8 deletions(-)

diff --git a/src/lib/axis/motor/servo/dc/DcServoDriver.h b/src/lib/axis/motor/servo/dc/DcServoDriver.h
index 0722a8ba7..6a914c221 100644
--- a/src/lib/axis/motor/servo/dc/DcServoDriver.h
+++ b/src/lib/axis/motor/servo/dc/DcServoDriver.h
@@ -102,18 +102,25 @@ class ServoDcDriver : public ServoDriver {
       sign = (zeroHoldSign < 0) ? -1 : 1;
     #endif
 
-    // Linear map: counts = countsMin + vAbs * gain
-    // fmaf keeps one rounding and is very fast on some FPUS(M7).
-    float countsF = fmaf(vAbs, velToCountsGain, (float)countsMinCached);
+    // Linear map WITHOUT offset
+    float countsF = vAbs * velToCountsGain; // target in [0 .. countsMaxCached]
 
     #ifdef SERVO_SIGMA_DELTA_DITHERING
-      // Dither the floating counts to an integer so the time-average equals countsF
-      int32_t power = sigmaDelta.dither_counts(countsF, countsMinCached, countsMaxCached);
+      // Dither the floating counts to an integer so the time-average equals countsF.
+      // Use 0..countsMaxCached as the dither bounds (not countsMinCached),
+      // then apply the minimum kick below.
+      int32_t power = sigmaDelta.dither_counts(countsF, 0, countsMaxCached);
     #else
       // Single float→int rounding at the end
       long power = (long)lroundf(countsF);
     #endif
 
+    // Enforce minimum only for non-zero outputs
+    if (power > 0 && power < countsMinCached) power = countsMinCached;
+
+    // Safety clamp to max
+    if (power > countsMaxCached) power = countsMaxCached;
+
     return power * sign;
   }
 
@@ -168,9 +175,9 @@ class ServoDcDriver : public ServoDriver {
         countsMaxCached = (int32_t)lroundf(maxCountsF);
         if (countsMaxCached < countsMinCached) countsMaxCached = countsMinCached; // safety
 
-        const int span = (int)(countsMaxCached - countsMinCached);
-        velToCountsGain = (velocityMaxCached > 0.0f) ? ((float)span / velocityMaxCached) : 0.0f;
-        VF("MSG:"); V(axisPrefix); VF("velToCountsGain="); V(velToCountsGain); VLF(" from velocity -> pwm units");
+        // gain: map velocity 0..Vmax → counts 0..countsMaxCached (no offset here)
+        velToCountsGain = (velocityMaxCached > 0.0f) ? ((float)countsMaxCached / velocityMaxCached) : 0.0f;
+        VF("MSG:"); V(axisPrefix); VF("velToCountsGain="); V(velToCountsGain); VLF(" (0..Vmax -> 0..pwm units)");
       }
     }
 

From 58b175ae182bbde8ba7f96d2feeb0161fc92d062 Mon Sep 17 00:00:00 2001
From: Panagiotis Papadakos <papadako@gmail.com>
Date: Fri, 31 Oct 2025 01:15:00 +0200
Subject: [PATCH 08/12] typo

---
 src/lib/axis/motor/servo/dc/DcServoDriver.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/axis/motor/servo/dc/DcServoDriver.h b/src/lib/axis/motor/servo/dc/DcServoDriver.h
index 6a914c221..348960a73 100644
--- a/src/lib/axis/motor/servo/dc/DcServoDriver.h
+++ b/src/lib/axis/motor/servo/dc/DcServoDriver.h
@@ -19,7 +19,7 @@
 #ifdef SERVO_HYSTERESIS_ENABLE
   // Thresholds in encoder counts/sec
   #ifndef SERVO_HYST_ENTER_CPS
-    #define SERVO_HYST_ENTER_CPS 20.0f   // must exceed this to LEAVE zero (1/10 of sidereal speed of 92 counts / sec)
+    #define SERVO_HYST_ENTER_CPS 20.0f   // must exceed this to LEAVE zero (~1/5 of sidereal speed of 92 counts / sec)
   #endif
   #ifndef SERVO_HYST_EXIT_CPS
     #define SERVO_HYST_EXIT_CPS 10.0f    // drop below this to RETURN to zero (half of the above)

From 736c49c5b0838e148e03a2dd928965bb393f2e41 Mon Sep 17 00:00:00 2001
From: Panagiotis Papadakos <papadako@gmail.com>
Date: Fri, 31 Oct 2025 01:30:43 +0200
Subject: [PATCH 09/12] servo DC: add more debugging messages for state of
 dithering / hysterisis

---
 src/lib/axis/motor/servo/dc/DcServoDriver.cpp | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/src/lib/axis/motor/servo/dc/DcServoDriver.cpp b/src/lib/axis/motor/servo/dc/DcServoDriver.cpp
index 2f8055dd4..d5d274d02 100644
--- a/src/lib/axis/motor/servo/dc/DcServoDriver.cpp
+++ b/src/lib/axis/motor/servo/dc/DcServoDriver.cpp
@@ -14,6 +14,19 @@ ServoDcDriver::ServoDcDriver(uint8_t axisNumber, const ServoPins *Pins, const Se
   this->pwmMaximum.valueDefault = pwmMaximum;
   // initialize caches up front
   recomputeScalingIfNeeded();
+
+  #ifdef SERVO_HYSTERESIS_ENABLE
+    VF("MSG:"); V(axisPrefix); VF("Hysteresis Enabled ENTER="); V((float)SERVO_HYST_ENTER_CPS);
+    VF(" cps, EXIT="); V((float)SERVO_HYST_EXIT_CPS); VLF(" cps");
+  #else
+    VF("MSG:"); V(axisPrefix); VLF("Hysteresis Disabled");
+  #endif
+
+  #ifdef SERVO_SIGMA_DELTA_DITHERING
+    VF("MSG:"); V(axisPrefix); VLF("Sigma-Delta dither: enabled");
+  #else
+    VF("MSG:"); V(axisPrefix); VLF("Sigma-Delta dither: disabled");
+  #endif
 }
 
 float ServoDcDriver::setMotorVelocity(float velocity)  {

From 02753881bded527beabc51071eb9227333301aeb Mon Sep 17 00:00:00 2001
From: Panagiotis Papadakos <papadako@gmail.com>
Date: Fri, 31 Oct 2025 01:44:35 +0200
Subject: [PATCH 10/12] servo DC: TODO note check with the calibration code
 whether the stiction breaking power can be reduced after the motor is moving

---
 src/lib/axis/motor/servo/dc/DcServoDriver.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/lib/axis/motor/servo/dc/DcServoDriver.h b/src/lib/axis/motor/servo/dc/DcServoDriver.h
index 348960a73..320872a5b 100644
--- a/src/lib/axis/motor/servo/dc/DcServoDriver.h
+++ b/src/lib/axis/motor/servo/dc/DcServoDriver.h
@@ -116,6 +116,8 @@ class ServoDcDriver : public ServoDriver {
     #endif
 
     // Enforce minimum only for non-zero outputs
+    // !TODO have to check with the calibration code whether the stiction breaking power can be reduced
+    // after the motor is moving
     if (power > 0 && power < countsMinCached) power = countsMinCached;
 
     // Safety clamp to max

From 15380bc124339d753bdb910aed05d96e39c0cd22 Mon Sep 17 00:00:00 2001
From: Panagiotis Papadakos <papadako@gmail.com>
Date: Fri, 31 Oct 2025 01:53:15 +0200
Subject: [PATCH 11/12] camelCase

---
 src/lib/axis/motor/servo/dc/DcServoDriver.h    | 2 +-
 src/lib/axis/motor/servo/dc/SigmaDeltaDither.h | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/lib/axis/motor/servo/dc/DcServoDriver.h b/src/lib/axis/motor/servo/dc/DcServoDriver.h
index 320872a5b..c50bce034 100644
--- a/src/lib/axis/motor/servo/dc/DcServoDriver.h
+++ b/src/lib/axis/motor/servo/dc/DcServoDriver.h
@@ -109,7 +109,7 @@ class ServoDcDriver : public ServoDriver {
       // Dither the floating counts to an integer so the time-average equals countsF.
       // Use 0..countsMaxCached as the dither bounds (not countsMinCached),
       // then apply the minimum kick below.
-      int32_t power = sigmaDelta.dither_counts(countsF, 0, countsMaxCached);
+      int32_t power = sigmaDelta.ditherCounts(countsF, 0, countsMaxCached);
     #else
       // Single float→int rounding at the end
       long power = (long)lroundf(countsF);
diff --git a/src/lib/axis/motor/servo/dc/SigmaDeltaDither.h b/src/lib/axis/motor/servo/dc/SigmaDeltaDither.h
index 3d250cddd..55cd74d38 100644
--- a/src/lib/axis/motor/servo/dc/SigmaDeltaDither.h
+++ b/src/lib/axis/motor/servo/dc/SigmaDeltaDither.h
@@ -24,7 +24,7 @@ struct SigmaDeltaDither {
 
     // convert a floating desired count into an integer count for this tick,
     // such that the time-average of outputs equals the floating target
-    inline int32_t dither_counts(float desiredCounts,
+    inline int32_t ditherCounts(float desiredCounts,
                                     int32_t minCount,
                                     int32_t maxCount)
     {

From 918741fd0b248014db22919a4b341f25dfda0bed Mon Sep 17 00:00:00 2001
From: Panagiotis Papadakos <papadako@gmail.com>
Date: Sun, 2 Nov 2025 19:14:59 +0200
Subject: [PATCH 12/12] servo(dc): tracking-only stiction kick when the motor
 was not moving or when changing direction. Implemented as a percentage over
 the min velocity

kick config: SERVO_STICTION_KICK_MS (related to mechanical and electrical time constants and inertia and the SERVO_STICTION_KICK_PERCENT_MULTIPLIER over the countsMin

- gate kick by tracking mode via ServoDriver::setTrackingMode(bool) (no-op in base, implemented in ServoDcDriver and called from ServoMotor::poll()).
- compute & cache countsBreakCached (SERVO_STICTION_KICK_PERCENT_MULTIPLIER) in recomputeScalingIfNeeded()
- Apply kick for SERVO_STICTION_KICK_MS on leave-zero or direction flip. After the SERVO_STICTION_KICK_MS time window then fall back to countsMinCached.
- Reset kick state on zero output and clamp to min/max counts.
---
 src/lib/axis/motor/servo/Servo.cpp          |   5 +-
 src/lib/axis/motor/servo/ServoDriver.h      |   3 +
 src/lib/axis/motor/servo/dc/DcServoDriver.h | 203 ++++++++++++++------
 3 files changed, 145 insertions(+), 66 deletions(-)

diff --git a/src/lib/axis/motor/servo/Servo.cpp b/src/lib/axis/motor/servo/Servo.cpp
index f1e9ec52c..a223bfc70 100644
--- a/src/lib/axis/motor/servo/Servo.cpp
+++ b/src/lib/axis/motor/servo/Servo.cpp
@@ -77,7 +77,7 @@ bool ServoMotor::init() {
   if (!driver->init(normalizedReverse)) { DF("ERR:"); D(axisPrefix); DLF("no motor driver!"); return false; }
 
   driver->enable(false);
-  
+
   // get the feedback control loop ready
   feedback->init(axisNumber, control);
   feedback->reset();
@@ -112,7 +112,7 @@ void ServoMotor::setReverse(int8_t state) {
   if (!ready) return;
 
   feedback->setControlDirection(state);
-  if (state == ON) encoderReverse = encoderReverseDefault; else encoderReverse = !encoderReverseDefault; 
+  if (state == ON) encoderReverse = encoderReverseDefault; else encoderReverse = !encoderReverseDefault;
 }
 
 void ServoMotor::enable(bool state) {
@@ -301,6 +301,7 @@ void ServoMotor::poll() {
   long unfilteredEncoderCounts = encoderCounts;
   UNUSED(unfilteredEncoderCounts);
   bool isTracking = (abs(currentFrequency - trackingFrequency) < trackingFrequency/10.0F);
+  driver->setTrackingMode(isTracking);
 
   encoderCounts = filter->update(encoderCounts, motorCounts, isTracking);
 
diff --git a/src/lib/axis/motor/servo/ServoDriver.h b/src/lib/axis/motor/servo/ServoDriver.h
index 61c09e5e5..3cb25ca0d 100644
--- a/src/lib/axis/motor/servo/ServoDriver.h
+++ b/src/lib/axis/motor/servo/ServoDriver.h
@@ -56,6 +56,9 @@ class ServoDriver {
     // enable or disable the driver using the enable pin or other method
     virtual void enable(bool state) { UNUSED(state); }
 
+    // let the driver know whether it is tracking or not
+    virtual void setTrackingMode(bool state) { UNUSED(state); }
+
     // sets overall maximum frequency
     // \param frequency: rate of motion in steps (counts) per second
     void setFrequencyMax(float frequency);
diff --git a/src/lib/axis/motor/servo/dc/DcServoDriver.h b/src/lib/axis/motor/servo/dc/DcServoDriver.h
index c50bce034..9025cd92d 100644
--- a/src/lib/axis/motor/servo/dc/DcServoDriver.h
+++ b/src/lib/axis/motor/servo/dc/DcServoDriver.h
@@ -19,7 +19,7 @@
 #ifdef SERVO_HYSTERESIS_ENABLE
   // Thresholds in encoder counts/sec
   #ifndef SERVO_HYST_ENTER_CPS
-    #define SERVO_HYST_ENTER_CPS 20.0f   // must exceed this to LEAVE zero (~1/5 of sidereal speed of 92 counts / sec)
+    #define SERVO_HYST_ENTER_CPS 20.0f   // must exceed this to LEAVE zero (e.g., ~1/5 of sidereal speed of 92 counts / sec)
   #endif
   #ifndef SERVO_HYST_EXIT_CPS
     #define SERVO_HYST_EXIT_CPS 10.0f    // drop below this to RETURN to zero (half of the above)
@@ -30,6 +30,19 @@
   #endif
 #endif
 
+// Stiction breakaway kick (ENABLE by defining SERVO_STICTION_KICK)
+#ifdef SERVO_STICTION_KICK
+  #ifndef SERVO_STICTION_KICK_MS
+    // duration of kick after zero->nonzero or direction flip
+    // depends on the mechanical time constant and electrical time constant of motors
+    #define SERVO_STICTION_KICK_MS 20
+  #endif
+
+  #ifndef SERVO_STICTION_KICK_PERCENT_MULTIPLIER
+    #define SERVO_STICTION_KICK_PERCENT_MULTIPLIER 3.50f // means 3.5x
+  #endif
+#endif
+
 #include "../ServoDriver.h"
 
 class ServoDcDriver : public ServoDriver {
@@ -48,83 +61,129 @@ class ServoDcDriver : public ServoDriver {
     // \returns velocity in effect, in encoder counts per second
     float setMotorVelocity(float velocity);
 
+    void setTrackingMode(bool state) {
+      #ifdef SERVO_STICTION_KICK
+        kickAllowedByMode = state;
+      #else
+        (void) state;
+      #endif
+    }
+
   protected:
     // convert from encoder counts per second to analogWriteRange units to roughly match velocity
     // we expect a linear scaling for the motors (maybe in the future we can be smarter?)
     // uses cached min/max counts and a precomputed gain
     // only float ops since usually there is no double FPU
-  long toAnalogRange(float velocity) {
-    // Update caches only if inputs changed
-    recomputeScalingIfNeeded();
-
-    // Extract sign and work with magnitude
-    int sign = 1;
-    if (velocity < 0.0F) { velocity = -velocity; sign = -1; }
+    long toAnalogRange(float velocity) {
+      // Update caches only if inputs changed
+      recomputeScalingIfNeeded();
+
+      // Extract sign and work with magnitude
+      int sign = 1;
+      if (velocity < 0.0F) { velocity = -velocity; sign = -1; }
+
+      if (velocity == 0.0F || velocityMaxCached <= 0.0f) {
+
+        #ifdef SERVO_HYSTERESIS_ENABLE
+          zeroHoldSign = 0; // ensure immediate exit and clear latch on exact zero
+        #endif
+
+        #ifdef SERVO_SIGMA_DELTA_DITHERING
+          sigmaDelta.reset(); // reset dithering residue when output is zero
+        #endif
+
+        // remember we are outputting zero
+        #ifdef SERVO_STICTION_KICK
+          lastPowerCounts = 0;
+          lastSign = 0;
+          kickUntilMs = 0;
+        #endif
+        return 0; // early out
+      }
 
-    if (velocity == 0.0F || velocityMaxCached <= 0.0f) {
+      // Clamp to velocityMax to avoid over-range math.
+      float vAbs = velocity; // already absolute
+      if (vAbs > velocityMaxCached) vAbs = velocityMaxCached;
 
       #ifdef SERVO_HYSTERESIS_ENABLE
-        zeroHoldSign = 0; // ensure immediate exit and clear latch on exact zero
+        // Hysteresis around zero: require a larger enter threshold to leave zero,
+        // and a smaller "exit" threshold to return to zero. This prevents chatter
+        // when the PID jitters around zero
+        if (zeroHoldSign == 0) {
+          // currently at zero: must exceed enter threshold to start moving
+          if (vAbs < SERVO_HYST_ENTER_CPS) return 0;
+          zeroHoldSign = (sign >= 0) ? 1 : -1; // latch direction
+        } else {
+          // currently moving: if we drop below exit threshold, snap back to zero
+          if (vAbs < SERVO_HYST_EXIT_CPS) {
+            zeroHoldSign = 0;
+            #ifdef SERVO_SIGMA_DELTA_DITHERING
+              sigmaDelta.reset(); // also reset when snapping back to zero
+            #endif
+            return 0;
+          }
+          // allow direction change if command flips while above thresholds
+          if ((sign >= 0 ? 1 : -1) != zeroHoldSign) zeroHoldSign = (sign >= 0) ? 1 : -1;
+        }
+        // use latched direction while moving
+        sign = (zeroHoldSign < 0) ? -1 : 1;
       #endif
 
+      // Linear map WITHOUT offset
+      float countsF = vAbs * velToCountsGain; // target in [0 .. countsMaxCached]
+
       #ifdef SERVO_SIGMA_DELTA_DITHERING
-        sigmaDelta.reset(); // reset dithering residue when output is zero
+        // Dither the floating counts to an integer so the time-average equals countsF.
+        // Use 0..countsMaxCached as the dither bounds (not countsMinCached),
+        // then apply the minimum kick below.
+        int32_t power = sigmaDelta.ditherCounts(countsF, 0, countsMaxCached);
+      #else
+        // Single float→int rounding at the end
+        long power = (long)lroundf(countsF);
       #endif
 
-      return 0; // early out
-    }
-
-    // Clamp to velocityMax to avoid over-range math.
-    float vAbs = velocity; // already absolute
-    if (vAbs > velocityMaxCached) vAbs = velocityMaxCached;
-
-    #ifdef SERVO_HYSTERESIS_ENABLE
-      // Hysteresis around zero: require a larger enter threshold to leave zero,
-      // and a smaller "exit" threshold to return to zero. This prevents chatter
-      // when the PID jitters around zero
-      if (zeroHoldSign == 0) {
-        // currently at zero: must exceed enter threshold to start moving
-        if (vAbs < SERVO_HYST_ENTER_CPS) return 0;
-        zeroHoldSign = (sign >= 0) ? 1 : -1; // latch direction
-      } else {
-        // currently moving: if we drop below exit threshold, snap back to zero
-        if (vAbs < SERVO_HYST_EXIT_CPS) {
-          zeroHoldSign = 0;
-          #ifdef SERVO_SIGMA_DELTA_DITHERING
-            sigmaDelta.reset(); // also reset when snapping back to zero
-          #endif
-          return 0;
+      #ifdef SERVO_STICTION_KICK
+        const uint32_t now = millis();
+        const int reqSign = (sign >= 0) ? +1 : -1;
+
+        if (kickAllowedByMode) {
+          // Start a stiction kick ?
+          // We kick when we are at rest (lastPowerCounts == 0) and receive a nonzero request,
+          // or when we flip direction.
+          bool leaveZero   = (lastPowerCounts == 0) && (power > 0);
+          bool dirFlip     = (lastSign != 0 && reqSign != lastSign);
+
+          if (leaveZero || dirFlip) {
+            kickUntilMs = now + SERVO_STICTION_KICK_MS;
+          }
+
+          // If we are within the kick window, enforce the breakaway minimum
+          if (kickUntilMs != 0 && (int32_t)(now - kickUntilMs) < 0) {
+            if (power > 0 && power < countsBreakCached) power = countsBreakCached;
+          } else {
+            kickUntilMs = 0; // window expired
+            if (power > 0 && power < countsMinCached) power = countsMinCached;
+          }
+        } else {
+          // Not in tracking mode: no kick, just sustaining minimum
+          kickUntilMs = 0;
+          if (power > 0 && power < countsMinCached) power = countsMinCached;
         }
-        // allow direction change if command flips while above thresholds
-        if ((sign >= 0 ? 1 : -1) != zeroHoldSign) zeroHoldSign = (sign >= 0) ? 1 : -1;
-      }
-      // use latched direction while moving
-      sign = (zeroHoldSign < 0) ? -1 : 1;
-    #endif
-
-    // Linear map WITHOUT offset
-    float countsF = vAbs * velToCountsGain; // target in [0 .. countsMaxCached]
-
-    #ifdef SERVO_SIGMA_DELTA_DITHERING
-      // Dither the floating counts to an integer so the time-average equals countsF.
-      // Use 0..countsMaxCached as the dither bounds (not countsMinCached),
-      // then apply the minimum kick below.
-      int32_t power = sigmaDelta.ditherCounts(countsF, 0, countsMaxCached);
-    #else
-      // Single float→int rounding at the end
-      long power = (long)lroundf(countsF);
-    #endif
 
-    // Enforce minimum only for non-zero outputs
-    // !TODO have to check with the calibration code whether the stiction breaking power can be reduced
-    // after the motor is moving
-    if (power > 0 && power < countsMinCached) power = countsMinCached;
-
-    // Safety clamp to max
-    if (power > countsMaxCached) power = countsMaxCached;
-
-    return power * sign;
-  }
+        // Final clamp and sign/update
+        if (power > countsMaxCached) power = countsMaxCached;
+        power *= reqSign;
+        lastPowerCounts = power;
+        lastSign        = (power > 0) ? +1 : (power < 0 ? -1 : 0);
+        return power;
+      #else
+        // No kick feature: just apply sustaining minimum and clamp
+        if (power > 0 && power < countsMinCached) power = countsMinCached;
+        if (power > countsMaxCached) power = countsMaxCached;
+        power *= (sign >= 0) ? +1 : -1;
+        return power;
+      #endif
+    }
 
     // motor control update
     virtual void pwmUpdate(long power) { }
@@ -150,7 +209,6 @@ class ServoDcDriver : public ServoDriver {
     int32_t countsMaxCached   = 0;      // integer max duty in counts
     float   velToCountsGain   = 0.0f;   // counts per (encoder count/s)
 
-    // Recompute cache if any dependency changed.
     inline void recomputeScalingIfNeeded() {
       const long analogMaxNow = (analogWriteRange - 1);
 
@@ -180,6 +238,15 @@ class ServoDcDriver : public ServoDriver {
         // gain: map velocity 0..Vmax → counts 0..countsMaxCached (no offset here)
         velToCountsGain = (velocityMaxCached > 0.0f) ? ((float)countsMaxCached / velocityMaxCached) : 0.0f;
         VF("MSG:"); V(axisPrefix); VF("velToCountsGain="); V(velToCountsGain); VLF(" (0..Vmax -> 0..pwm units)");
+
+        #ifdef SERVO_STICTION_KICK
+          // Breakaway counts = max(min, min * EXTRA_PCT), clamped to max
+          float breakF = fmaxf(minCountsF, minCountsF * SERVO_STICTION_KICK_PERCENT_MULTIPLIER);
+          int32_t breakCounts = (int32_t)lroundf(breakF);
+          if (breakCounts > countsMaxCached) breakCounts = countsMaxCached;
+          countsBreakCached = breakCounts;
+          VF("MSG:"); V(axisPrefix); VF("breakaway counts="); V(countsBreakCached); VLF("");
+        #endif
       }
     }
 
@@ -187,6 +254,14 @@ class ServoDcDriver : public ServoDriver {
       int8_t zeroHoldSign = 0; // 0: at zero; +1 / -1: direction while "moving"
     #endif
 
+    #ifdef SERVO_STICTION_KICK
+      // --- breakaway kick state ---
+      int32_t  lastPowerCounts = 0;   // last output after clamping (signed)
+      int8_t   lastSign        = 0;   // -1, 0, +1 of lastPowerCounts
+      uint32_t kickUntilMs     = 0;   // time until which we keep kicking
+      int32_t  countsBreakCached = 0; // breakaway minimum in counts (recomputed with scaling)
+      bool kickAllowedByMode = false;  // set via setTrackingMode()
+    #endif
 };
 
 #endif