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