From 9788c944c44b5e1623618592b3ffc2d41d9c4469 Mon Sep 17 00:00:00 2001
From: BlueAndi <web@blue-andi.de>
Date: Fri, 3 Nov 2023 13:19:43 +0100
Subject: [PATCH] Bugfix: Wrap around handling in the relative encoders is
 automatically applied, no extra algorithm is necessary.

---
 lib/Service/RelativeEncoders.cpp | 19 ++-----------------
 lib/Service/RelativeEncoders.h   | 11 -----------
 2 files changed, 2 insertions(+), 28 deletions(-)

diff --git a/lib/Service/RelativeEncoders.cpp b/lib/Service/RelativeEncoders.cpp
index 260aea33..3f4d0510 100644
--- a/lib/Service/RelativeEncoders.cpp
+++ b/lib/Service/RelativeEncoders.cpp
@@ -76,12 +76,12 @@ void RelativeEncoders::clearRight()
 
 int16_t RelativeEncoders::getCountsLeft() const
 {
-    return calculate(m_absEncoders.getCountsLeft(), m_referencePointLeft);
+    return m_absEncoders.getCountsLeft() - m_referencePointLeft;
 }
 
 int16_t RelativeEncoders::getCountsRight() const
 {
-    return calculate(m_absEncoders.getCountsRight(), m_referencePointRight);
+    return m_absEncoders.getCountsRight() - m_referencePointRight;
 }
 
 /******************************************************************************
@@ -92,21 +92,6 @@ int16_t RelativeEncoders::getCountsRight() const
  * Private Methods
  *****************************************************************************/
 
-int16_t RelativeEncoders::calculate(int16_t absSteps, int16_t refPoint) const
-{
-    int16_t diffSteps = absSteps - refPoint;
-
-    /* Wrap around in forward or backward direction?
-     * In both cases the difference must be multiplied with -1.
-     */
-    if (((0 > absSteps) && (0 < refPoint)) || ((0 < absSteps) && (0 > refPoint)))
-    {
-        diffSteps *= -1;
-    }
-
-    return diffSteps;
-}
-
 /******************************************************************************
  * External Functions
  *****************************************************************************/
diff --git a/lib/Service/RelativeEncoders.h b/lib/Service/RelativeEncoders.h
index e69b5d0c..a8807e65 100644
--- a/lib/Service/RelativeEncoders.h
+++ b/lib/Service/RelativeEncoders.h
@@ -132,17 +132,6 @@ class RelativeEncoders
     RelativeEncoders();
     RelativeEncoders(const RelativeEncoders& relEncoder);
     RelativeEncoders& operator=(const RelativeEncoders& relEncoder);
-
-    /**
-     * Calculate relative number of encoder steps between the reference point
-     * and the current absolute number of encoder steps.
-     * 
-     * @param[in] absSteps  Absolute number of encoder steps
-     * @param[in] refPoint  Reference point
-     * 
-     * @return Relative number of encoder steps
-     */
-    int16_t calculate(int16_t absSteps, int16_t refPoint) const;
 };
 
 /******************************************************************************