From d0e0a2ade316d3200912b9525f7acf3f7a9e5492 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sebastian=20B=C3=B6ttger?=
 <sebastian.boettger@stud.hs-bochum.de>
Date: Tue, 20 Jun 2023 16:25:22 +0200
Subject: [PATCH] Fixing problem with odometrie

---
 driving.py | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/driving.py b/driving.py
index b0ce6e0..58f88b2 100644
--- a/driving.py
+++ b/driving.py
@@ -78,12 +78,16 @@ class Driving_Analyser:
         
         if w != 0 or v != 0:
             theta = self.theta_old + (w * time_delta)
-            y = self.y_old + (v * (-np.cos(theta) / w))
+           #y = self.y_old + (v * ((-np.cos(theta) / w) + 1))
+            if w < 0:
+                y = self.y_old + (v * (-np.cos(theta) / w - 1))
+            else:
+                y = self.y_old + (v * (-np.cos(theta) / w + 1))
             x =  self.x_old + (v * (np.sin(theta) / w))
             
             self.theta_old = theta
-            self.y_old = y
-            self.x_old = x
+            self.y_old = round(y, 5)
+            self.x_old = round(x, 5)
         
         return (self.x_old, self.y_old, self.theta_old)
     
-- 
GitLab