From 2c58777e45a431e88c3e01854e369187389641f2 Mon Sep 17 00:00:00 2001
From: Bernhard Klauninger <bernhard@klauninger.at>
Date: Wed, 15 Mar 2023 13:14:06 +0100
Subject: [PATCH] Actually fixed wrap around

---
 src/create.cpp | 22 +++++++++++-----------
 1 file changed, 11 insertions(+), 11 deletions(-)

diff --git a/src/create.cpp b/src/create.cpp
index f6bd595..87c12c9 100644
--- a/src/create.cpp
+++ b/src/create.cpp
@@ -157,18 +157,18 @@ namespace create {
       int ticksLeft = totalTicksLeft - prevTicksLeft;
       int ticksRight = totalTicksRight - prevTicksRight;
 
-      // Handle wrap around
-      if (ticksLeft > 0.9 * util::V_3_MAX_ENCODER_TICKS) {
-        ticksLeft = 2 * util::V_3_MAX_ENCODER_TICKS + prevTicksLeft - ticksLeft;
-      } else if (ticksLeft < 0.9 * -util::V_3_MAX_ENCODER_TICKS) {
-        ticksLeft = util::V_3_MAX_ENCODER_TICKS - prevTicksLeft + util::V_3_MAX_ENCODER_TICKS + ticksLeft;
-      }
+    // Handle wrap around
+    if (ticksLeft > 0.9 * util::V_3_MAX_ENCODER_TICKS) {
+        ticksLeft -= util::V_3_MAX_ENCODER_TICKS;
+    } else if (ticksLeft < -0.9 * util::V_3_MAX_ENCODER_TICKS) {
+        ticksLeft += util::V_3_MAX_ENCODER_TICKS;
+    }
 
-      if (ticksRight > 0.9 * util::V_3_MAX_ENCODER_TICKS) {
-        ticksRight = util::V_3_MAX_ENCODER_TICKS + prevTicksRight + util::V_3_MAX_ENCODER_TICKS - ticksRight;
-      } else if (ticksLeft < 0.9 * -util::V_3_MAX_ENCODER_TICKS) {
-        ticksRight = util::V_3_MAX_ENCODER_TICKS - prevTicksRight + util::V_3_MAX_ENCODER_TICKS + ticksRight;
-      }
+    if (ticksRight > 0.9 * util::V_3_MAX_ENCODER_TICKS) {
+        ticksRight -= util::V_3_MAX_ENCODER_TICKS;
+    } else if (ticksRight < -0.9 * util::V_3_MAX_ENCODER_TICKS) {
+        ticksRight += util::V_3_MAX_ENCODER_TICKS;
+    }
 
       prevTicksLeft = totalTicksLeft;
       prevTicksRight = totalTicksRight;