derped/robolab
1
0
Fork 0
Code Pull requests Releases Activity

Fixed odometry methods.

Browse source
This commit is contained in:
jureao 2018-03-24 18:49:13 +01:00
parent 3112046256
commit cdb6ddc683
1 changed files with 4 additions and 3 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

7
src/odometry.py
View file

@ -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)