-
Notifications
You must be signed in to change notification settings - Fork 0
/
Car.py
158 lines (125 loc) · 5.54 KB
/
Car.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
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
import lib601.sm as sm
import time
import pigpio
import threading
import Queue
from motor import motor
from sonar import ranger
from encoder import encoder
class Car(sm.SM, threading.Thread):
def __init__(self, pi, motor_left, motor_right, encoder_left, encoder_right, sonar, encoder_queue, sonar_queue):
self.motor_left = motor_left
self.motor_right = motor_right
self.encoder_left = encoder_left
self.encoder_right = encoder_right
self.sonar = sonar
self.pi = pi
self.sonar_queue = sonar_queue
self.encoder_queue = encoder_queue
threading.Thread.__init__(self)
def parallel_test(self): #Test to verify usdist_sensor callback is working
x = []
y = []
th_sonars = []
th_encoders = []
counter = 0
to = time.time()
started = False
end = 100
t_enc = 1
self.motor_left.speed(200)
self.motor_right.speed(220) #offset of 20 required to get straight line...
while len(x) < end:
if not started: #Check if there is a sonar thread and if there is any data in the queue.
started = True
th_sonar = threading.Thread(target=self.sonar.read)
th_sonar.setDaemon(False)
th_sonar.start()
th_sonars += [th_sonar] #Make list of all sonar threads created. Will all be joined.
th_encoder = threading.Thread(target=self.encoder_left.distance, kwargs={'t': t_enc})
th_encoder.setDaemon(False)
th_encoder.start()
th_encoders += [th_encoder] #Make list of all encoder threads created. Will all be joined.
else:
if not th_sonar.isAlive() and self.sonar_queue.empty(): #Make new sonar thread if it is data retrieved from last result
th_sonar = threading.Thread(target=self.sonar.read)
th_sonar.setDaemon(False)
th_sonar.start()
th_sonars += [th_sonar]
if not th_encoder.isAlive() and self.encoder_queue.empty(): #Make new encoder thread if data is retrieved from last result
th_encoder = threading.Thread(target=self.encoder_left.distance, kwargs={'t': t_enc})
th_encoder.setDaemon(False)
th_encoder.start()
th_encoders += [th_encoder] # Make list of all encoder threads created. Will all be joined.
#Check if the sonar queue is empty. Get data if it is not. Will not create another thread if queue is not empty
if not self.sonar_queue.empty():
x.append(self.sonar_queue.get(True, 1))
self.sonar_queue.task_done()
if not self.encoder_queue.empty():
y.append(self.encoder_queue.get(True, 1))
self.encoder_queue.task_done()
counter += 1
print "Total Time run: " + str(time.time() - to)
out = 0
self.sonar_thread_cleanup(th_sonars)
self.encoder_thread_cleanup(th_encoders, t_enc)
return (counter, x, y)
def sonar_thread_cleanup(self, threads):
for i in threads:
if not i.isAlive():
i.join()
else:
print "Thread " + str(i) + " is alive."
def encoder_thread_cleanup(self, threads, timer):
for i in threads:
if not i.isAlive():
i.join()
else:
print str(i) + " never joined. Waiting " + str(timer) + " seconds to try again."
time.sleep(timer)
if i.isAlive():
i.join()
else:
print str(i) + " WILL NOT JOIN!"
def cleanup(self):
self.motor_left.speed(0)
self.motor_right.speed(0)
print "Motors turned off"
while not self.sonar_queue.empty(): #Empty the queue and let queue know tasks are done.
self.sonar_queue.get()
self.sonar_queue.task_done()
if self.sonar_queue.empty(): #Join queue if it is empty
self.sonar_queue.join()
print "Sonar Queue joined"
else:
print "Sonar Queue not emptied. Can not join."
while not self.encoder_queue.empty():
self.encoder_queue.get()
self.encoder_queue.task_done()
if self.encoder_queue.empty():
self.encoder_queue.join()
print "Encoder Queue joined"
else:
print "Encoder Queue not emptied. Can not join"
self.sonar.cancel()
print "Sonar Cancelled"
self.pi.stop()
print "pigpio connection stopped"
def par_test():
print "Number of active threads before starting: " + str(threading.activeCount())
print "Active threads before starting: " + str(threading.enumerate())
pi = pigpio.pi()
sonar_queue = Queue.Queue()
encoder_queue = Queue.Queue()
sonar = ranger(pi=pi, trigger=17, echo=27, queue=sonar_queue)
enc = encoder(pi=pi, signal_pin=2, queue=encoder_queue)
motor_left = motor(pi=pi, forward_pin=26, back_pin=19)
motor_right = motor(pi=pi, forward_pin=16, back_pin=20)
car = Car(pi=pi, motor_left=motor_left, motor_right=motor_right, encoder_left=enc, encoder_right=enc, sonar=sonar, encoder_queue=encoder_queue, sonar_queue=sonar_queue)
result = car.parallel_test()
print "Counts: " + str(result[0])
print "Sonar Readings: " + str(result[1])
print "Encoder Readings: " + str(result[2])
car.cleanup()
if __name__ == '__main__':
pass