diff --git a/driving.py b/driving.py index a06e85401c7abc2a328c5c906dc97663330d9507..9d952dc92fa70edcd7ee6528f48fb4c1904a2618 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