RasPiBot202/odometer.py at master · DrGFreeman/RasPiBot202 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
import time
from math import pi, cos, sin

# Function boundAngle(angle) takes any angle as "angle" and returns the
# equivalent angle bound within 0 <= angle < 2 * Pi
def boundAngle(angle):
    return angle % (2 * pi)

# Function relativeAngle(angleRef, angle) returns the shortest relative
# angle from a reference angle "angleRef" to an angle "angle". The retuned
# relative angle is bound within -Pi < angle < Pi
def relativeAngle(angleRef, angle):
    angleRef = boundAngle(angleRef)
    angle = boundAngle(angle)

    if angle - angleRef > pi:
        relativeAngle = angle - angleRef - 2 * pi
    elif angle - angleRef < -pi:
        relativeAngle = angle - angleRef + 2 * pi
    else:
        relativeAngle = angle - angleRef

    return relativeAngle


class Odometer:

    def __init__(self, encoders, timeStep = .02):
        self.encoders = encoders
        self.timeStep = timeStep
        self.track = 142.5 # width between wheels in millimeters
        self.tickDist = .152505 # Distance travelled for per encoder click in millimeters
        self.lastCountLeft = 0
        self.lastCountRight = 0
        self.speedL = 0
        self.speedR = 0
        self.phi = 0
        self.x = 0
        self.y = 0
        self.v = 0
        self.omega = 0
        self.dist = 0
        self.active = False

    def update(self):

        countLeft, countRight = self.encoders.readCounts()

        deltaCountLeft = countLeft - self.lastCountLeft
        deltaCountRight = countRight - self.lastCountRight

        distLeft = deltaCountLeft * self.tickDist
        distRight = deltaCountRight * self.tickDist
        distCenter = (distLeft + distRight) / 2.
        self.dist += distCenter

        self.x += distCenter * cos(self.phi)
        self.y += distCenter * sin(self.phi)

        deltaPhi = (distRight - distLeft) / self.track
        self.phi = boundAngle(self.phi + deltaPhi)

        self.speedL = distLeft / self.timeStep
        self.speedR = distRight / self.timeStep
        self.v = distCenter / self.timeStep
        self.omega = deltaPhi / self.timeStep

        self.lastCountLeft = countLeft
        self.lastCountRight = countRight

    def getPosXY(self):
        return self.x, self.y

    def getPosXYPhi(self):
        return self.x, self.y, self.phi

    def getPhi(self):
        return self.phi

    def angleRelToPhi(self, angle):
        return relativeAngle(self.phi, angle)

    def getOmega(self):
        return self.omega

    def getSpeed(self):
        return self.v

    def getSpeedLR(self):
        return self.speedL, self.speedR

    def resetDist(self):
        self.dist = 0

    def resetEncoders(self):
        self.encoders.reset()
        self.lastCountLeft = 0
        self.lastCountRight = 0

    def resetPosXY(self):
        self.x = 0
        self.y = 0

    def resetPosXYPhi(self):
        self.phi = 0
        self.x = 0
        self.y = 0