84 lines
3.2 KiB
Python
84 lines
3.2 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
|
|
import decimal
|
|
|
|
'''
|
|
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
|
|
'''
|
|
|
|
|
|
def angle2dir(angle):
|
|
if angle % (2 * math.pi) >= 0 and angle % (2 * math.pi) < math.pi * 0.25 or angle % (2 * math.pi) >= 7 / 4 * math.pi and angle % (2 * math.pi) < 2 * math.pi:
|
|
return Direction.NORTH
|
|
elif angle % (2 * math.pi) >= math.pi * 0.25 and angle % (2 * math.pi) < math.pi * 0.5 or angle % (2 * math.pi) >= math.pi * 0.5 and angle % (2 * math.pi) < math.pi * 0.75:
|
|
return Direction.EAST
|
|
elif angle % (2 * math.pi) >= math.pi * 0.75 and angle % (2 * math.pi) < math.pi or angle % (2 * math.pi) >= math.pi and angle % (2 * math.pi) < 5 / 4 * math.pi:
|
|
return Direction.SOUTH
|
|
else:
|
|
return Direction.WEST
|
|
|
|
|
|
def posround(pos):
|
|
return decimal.Decimal(pos/50).quantize(0, rounding=decimal.ROUND_HALF_UP)
|
|
|
|
|
|
class Odometry:
|
|
def __init__(self): # planet
|
|
self._wheel_axis = 11
|
|
self._distance_per_tick = 0.0488
|
|
self._pi = math.pi
|
|
self._posxy = [0, 0]
|
|
self._distance = [0, 0]
|
|
self._wheel_center = 0
|
|
self._v_length = 0
|
|
self._v_angle = math.radians(Direction.NORTH)
|
|
|
|
def reset(self):
|
|
self._distance = [0, 0]
|
|
self._v_length = 0
|
|
self._posxy = [0, 0]
|
|
|
|
def angle_set(self, newangle):
|
|
self._v_angle = math.radians(newangle)
|
|
|
|
def angle_get(self):
|
|
return angle2dir(self._v_angle)
|
|
|
|
def coordinates_get(self):
|
|
return ((posround(self._posxy[0]), posround(self._posxy[1])), angle2dir(self._v_angle))
|
|
|
|
def pos_update(self, ticks_wheel):
|
|
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 + (self._distance[0] - self._distance[1])/self._wheel_axis
|
|
self._wheel_center = (self._distance[0] + self._distance[1])/2
|
|
self.coordinates_update()
|
|
|
|
def coordinates_update(self): # worked as function outside the class
|
|
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)
|
|
|
|
def dir2ticks(self, destdir): # return amount of ticks to turn to a given direction (current direction should be _v_angle)
|
|
return (destdir - (math.degrees(self._v_angle) % 360)) * 2
|