From 6b892b922fcad092e8c5847fedd45248bc443bfb 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:29:41 +0200
Subject: [PATCH] Fixing problem with odometrie
---
driving.py | 6 ++++--
1 file changed, 4 insertions(+), 2 deletions(-)
diff --git a/driving.py b/driving.py
index 58f88b2..cd43f4d 100644
--- a/driving.py
+++ b/driving.py
@@ -78,7 +78,6 @@ 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) + 1))
if w < 0:
y = self.y_old + (v * (-np.cos(theta) / w - 1))
else:
@@ -97,7 +96,10 @@ class Driving_Analyser:
"""
def calculate_odometrie_delta (self, w,v,time_delta):
theta_delta = (w * time_delta)
- y_delta = (v * (-np.cos(theta_delta) / w))
+ if w < 0:
+ y_delta = (v * (-np.cos(theta_delta) / w - 1))
+ else:
+ y_delta = (v * (-np.cos(theta_delta) / w + 1))
x_delta = (v * (np.sin(w * time_delta) / w))
return (x_delta, y_delta, theta_delta)
--
GitLab