From cdb6ddc6830afef76fd3c67be4c7959cc5809fa1 Mon Sep 17 00:00:00 2001 From: jureao Date: Sat, 24 Mar 2018 18:49:13 +0100 Subject: [PATCH] Fixed odometry methods. --- src/odometry.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/odometry.py b/src/odometry.py index f3dce02..d151c48 100644 --- a/src/odometry.py +++ b/src/odometry.py @@ -67,13 +67,14 @@ class Odometry: self._distance[0] = ticks_wheel[0] * self._distance_per_tick self._distance[1] = ticks_wheel[1] * self._distance_per_tick self._v_length = self._v_length + (self._distance[0] + self._distance[1])/2 - self._v_angle = self._v_angle + (ticks_wheel[0] - ticks_wheel[1])/self._wheel_axis + self._v_angle = self._v_angle + (self._distance[0] - self._distance[1])/self._wheel_axis self.coordinates_update() + self._wheel_center = self._distance[0] + self._distance[1])/2 print((self._v_length, angle2dir(self._v_angle))) def coordinates_update(self): # worked as function outside the class - self._posxy[0] = self._posxy[0] + self._v_length * math.sin(self._v_angle) - self._posxy[1] = self._posxy[1] + self._v_length * math.cos(self._v_angle) + self._posxy[0] = self._posxy[0] + self._wheel_center * math.sin(self._v_angle) + self._posxy[1] = self._posxy[1] + self._wheel_center * math.cos(self._v_angle) print((self._posxy[0], self._posxy[1])) def dir2ticks(self, destdir): # return amount of ticks to turn to a given direction (current direction should be _v_angle)