diff --git a/driving.py b/driving.py index 58f88b2169fa17d9d5d8bb7235c61ab2d0fb19ab..cd43f4d7fb5f78aa80783032d4e18df022d0021e 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)