From 51d6bc799d165d403eca32c4a7880c29036c95db Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sebastian=20B=C3=B6ttger?=
<sebastian.boettger@stud.hs-bochum.de>
Date: Thu, 29 Jun 2023 10:36:24 +0200
Subject: [PATCH] Fixing odometrie problem
---
driving.py | 13 ++++++-------
1 file changed, 6 insertions(+), 7 deletions(-)
diff --git a/driving.py b/driving.py
index a06e854..9d952dc 100644
--- a/driving.py
+++ b/driving.py
@@ -11,7 +11,6 @@ import numpy as np
S = 0 # Value for a straight part of the track
R = 1 # Value for a right turn
L = 2 # Value for a left turn
-
#%% Classes
"""
# This class contains some functions to analyze data from the LEGO robot EV3.
@@ -86,13 +85,13 @@ class Driving_Analyser:
else:
theta = self.theta_old
- x = self.x_old + (v * np.cos(self.theta_old) * time_delta)
- y = self.y_old + (v * np.sin(self.theta_old) * time_delta)
+ x = self.x_old + (v * np.cos(theta) * time_delta)
+ y = self.y_old + (v * np.sin(theta) * time_delta)
- self.theta_old = theta
- self.y_old = round(y, 5)
- self.x_old = round(x, 5)
+ self.theta_old = theta
+ self.y_old = round(y, 5)
+ self.x_old = round(x, 5)
return (self.x_old, self.y_old, self.theta_old)
@@ -105,7 +104,7 @@ class Driving_Analyser:
if w != 0:
theta_delta = (w * time_delta)
x_delta = (v * (np.sin(theta_delta) / w))
- y_delta = (v * (-(np.cos(theta_delta) + cos(0)) / w))
+ y_delta = (v * (-(np.cos(theta_delta) - np.cos(0)) / w))
else:
theta_delta = 0
--
GitLab