-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy path__vire.py
133 lines (102 loc) · 3.5 KB
/
__vire.py
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
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
from gpiozero import DigitalInputDevice
from threading import Thread
from time import sleep
from math import *
from time import sleep
from BMI160_i2c import Driver
class OldPositionWatcher:
perimeter = 60*pi
theta = pi / 2
#theta = (0, 0)
x = 0
y = 0
# left
phaseA = DigitalInputDevice(6, True)
phaseB = DigitalInputDevice(16, True)
# right
phaseC = DigitalInputDevice(20, True)
phaseD = DigitalInputDevice(21, True)
leftTicks = 0
rightTicks = 0
leftState = (0, 0)
leftOldState = (0, 0)
rightState = (0, 0)
rightOldState = (0, 0)
watchPositionThread = None
watchTicksThread = None
enabled = True
oldTicks = (0, 0)
onPositionChangedHandler = None
isTurning = False
L = 195
l = 215
def watchTicks(self):
while self.enabled:
leftFetchedState = (self.phaseA.value, self.phaseB.value)
rightFetchedState = (self.phaseC.value, self.phaseD.value)
if leftFetchedState != self.leftState:
self.leftState = leftFetchedState
if self.leftState[0] == self.leftOldState[1]:
self.leftTicks -= 1
else:
self.leftTicks += 1
self.leftOldState = self.leftState
if rightFetchedState != self.rightState:
self.rightState = rightFetchedState
if self.rightState[0] == self.rightOldState[1]:
self.rightTicks -= 1
else:
self.rightTicks += 1
self.rightOldState = self.rightState
def watchPosition(self):
while self.enabled:
# LEFT = SIDE
# RIGHT = BACK
newTicks = (self.leftTicks, self.rightTicks)
if (newTicks != self.oldTicks):
deltaTicks = (newTicks[0] - self.oldTicks[0],
newTicks[1] - self.oldTicks[1])
self.oldTicks = newTicks
leftDistance = deltaTicks[0] / 2400 * self.perimeter
rightDistance = deltaTicks[1] / 2400 * self.perimeter
# t1 = (leftDistance + rightDistance) / 2
# alpha = (rightDistance - leftDistance) / self.axialDistance
self.x += sin(self.theta)*-rightDistance + cos(self.theta)*leftDistance
self.y += cos(self.theta)*rightDistance + sin(self.theta)*leftDistance
if self.onPositionChangedHandler != None:
self.onPositionChangedHandler(self.x, self.y, self.theta)
#sleep(0.01)
# def watchOrientation(self):
# sensor = Driver()
# sensor.autoCalibrateGyroOffset()
# print("Gyro - Calibration done!")
# timeInterval = 0.05
# self.theta = pi/2
# while (self.enabled):
# sleep(timeInterval)
# self.theta += radians(((sensor.getRotationZ()[0] * 250.0) / 32768.0) * timeInterval)
def start(self):
self.enabled = True
self.watchTicksThread = Thread(target=self.watchTicks)
self.watchTicksThread.start()
self.watchPositionThread = Thread(target=self.watchPosition)
self.watchPositionThread.start()
# self.watchOrientationThread = Thread(target=self.watchOrientation)
# self.watchOrientationThread.start()
def stop(self):
self.enabled = False
def getPos(self):
return (self.x, self.y)
def getPosRot(self):
return (self.x, self.y, self.theta * 180/pi)
def changeIsTurning(self, p):
self.isTurning = p
return self.isTurning
def getOrientation(self):
return (self.theta)
def getOrientationDeg(self):
return (self.theta * 180/pi)
def setOnPositionChangedHandler(self, handler):
self.onPositionChangedHandler = handler
def getTicks(self):
return [self.leftTicks, self.rightTicks]