63 lines
3.0 KiB
Python
63 lines
3.0 KiB
Python
#!/usr/bin/env python3
|
|
# Suggestion: implement odometry as class that is not using the ev3dev.ev3 package
|
|
# establish value exchange with main driving class via getters and setters
|
|
|
|
import time
|
|
from planet import Direction
|
|
import math
|
|
|
|
'''
|
|
Documentation:
|
|
good and simple explanation on how odometry works:
|
|
https://www.youtube.com/watch?v=qsdiIZncgqo
|
|
|
|
Summary:
|
|
axis a (measure distance where wheels have most friction)
|
|
calibration factor c = diameter/ticks_per_turn
|
|
distance d = ticks wheel * c (calculate for each wheel individually)
|
|
vector_length = (d_l + d_r)/2
|
|
vector_angle ~= (d_l + d_r)/a (only applicable for small distances)
|
|
|
|
Functionality that should be implemented in this class:
|
|
- tracking of relative distances
|
|
- track the roboters orientation
|
|
- maybe have seperate functions for vector length and orientation (only if it is beneficial)
|
|
- closest coordinate to relative distance estimation/calculation
|
|
'''
|
|
|
|
|
|
class Odometry:
|
|
def __init__(self): # planet
|
|
self._wheel_axis = 11
|
|
self._distance_per_tick = 0.0488
|
|
self._pi = 3.141
|
|
self.distance = 0
|
|
self.alpha = 0
|
|
|
|
|
|
# worked fine outside the class as function in nano, maybe we must fix it a bit :)
|
|
def degree(self, delta_rotation_l, delta_rotation_r): # delta_rotation_l/r should work with getmovement method
|
|
|
|
self.alpha = ((delta_rotation_l * self._distance_per_tick - delta_rotation_r * self._distance_per_tick)/self._wheel_axis) + self.alpha
|
|
self.distance = (delta_rotation_r * self._distance_per_tick + delta_rotation_l * self._distance_per_tick)/2 + self.distance
|
|
if self.alpha % (2 * self._pi) >= 0 and self.alpha % (2 * self._pi) < self._pi * 0.25 or self.alpha % (2 * self._pi) >= 7 / 4 * self._pi and self.alpha % (2 * self._pi) < 2 * self._pi:
|
|
return(Direction.NORTH, "Distance = ", self.distance)
|
|
|
|
elif self.alpha % (2 * self._pi) >= self._pi * 0.25 and self.alpha % (2 * self._pi) < self._pi * 0.5 or self.alpha % (2 * self._pi) >= self._pi * 0.5 and self.alpha % (2 * self._pi) < self._pi * 0.75:
|
|
return(Direction.EAST, "Distance = ", self.distance)
|
|
|
|
elif self.alpha % (2 * self._pi) >= self._pi * 0.75 and self.alpha % (2 * self._pi) < self._pi or self.alpha % (2 * self._pi) >= self._pi and self.alpha % (2 * self._pi) < 5 / 4 * self._pi:
|
|
return(Direction.SOUTH, "Distance = ", self.distance)
|
|
|
|
else:
|
|
return(Direction.WEST, "Distance = ", self.distance)
|
|
|
|
def coordinates(self, delta_rota_l, delta_rota_r, Y_koord, X_koord): # worked as function outside the class
|
|
self.y = self.Y_koord
|
|
self.x = self.X_koord
|
|
self.wheel_center = (delta_rota_r + delta_rota_l) / 2
|
|
self.alpha = ((self.delta_rotation_l * self._distance_per_tick - self.delta_rotation_r * self._distance_per_tick)/self._wheel_axis) + self.alpha
|
|
self.y = self.y + self.wheel_center * math.cos(self.alpha)
|
|
self.x = self.x + self.wheel_center * math.sin(self.alpha)
|
|
return(self.x, self.y)
|