-
Notifications
You must be signed in to change notification settings - Fork 0
/
CamControl_old.py
253 lines (218 loc) · 6.52 KB
/
CamControl_old.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
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
import time
from multiprocessing import Process, Queue
class CamControl:
#Upper Limit for servos
_ServoXul = 250
_ServoYul = 230
#Lower limit for servos
_ServoXll = 75
_ServoYll = 75
"""
Servos are default to Servo X axis is assigned (servo-0) GPIO 4
Servo Y axis is assigned (servo-0) GPIO 17
"""
def __init__(self):
#ServoBlaster is what we use to control servo motors
self.servoBlaster = open('/dev/servoblaster', 'w')
"""
self.servo_X = Servo(pi, servoX_pin)
self.servo_Y = Servo(pi, servoY_pin)
"""
self.ServoXcp = Queue()
self.ServoXdp = Queue()
self.ServoXs = Queue()
self.ServoYcp = Queue()
self.ServoYdp = Queue()
self.ServoYs = Queue()
self._ServoXcp = 99
self._ServoXdp = 100
self._ServoYcp = 99
self._ServoYdp = 100
def recenter( self, offset_x, offset_y ):
#TODO
print("Centering camera...")
def camRight(self, distance, speed):
print "right"
if not self.ServoXcp.empty():
self._ServoXcp = self.ServoXcp.get()
self._ServoXdp = self._ServoXcp + distance
if self._ServoXdp > CamControl._ServoXul:
self._ServoXdp = CamControl._ServoXul
self.ServoXdp.put(self._ServoXdp)
self.ServoXs.put(speed)
return;
def camLeft(self, distance, speed):
print "left"
if not self.ServoXcp.empty():
self._ServoXcp = self.ServoXcp.get()
self._ServoXdp = self._ServoXcp - distance
if self._ServoXdp < CamControl._ServoXll:
self._ServoXdp = CamControl._ServoXll
self.ServoXdp.put(self._ServoXdp)
self.ServoXs.put(speed)
return;
def camUp(self, distance, speed):
print "up"
if not self.ServoYcp.empty():
self._ServoYcp = self.ServoYcp.get()
self._ServoYdp = self._ServoYcp - distance
if self._ServoYdp < CamControl._ServoYul:
self._ServoYdp = CamControl._ServoYul
self.ServoYdp.put(self._ServoYdp)
self.ServoYs.put(speed)
return;
def camDown(self, distance, speed):
print "down"
if not self.ServoYcp.empty():
self._ServoYcp = self.ServoYcp.get()
self._ServoYdp = self._ServoYcp - distance
if self._ServoYdp < CamControl._ServoYll:
self._ServoYdp = CamControl._ServoYll
self.ServoYdp.put(self._ServoYdp)
self.ServoYs.put(speed)
return;
# process X controls servoX
def PX(self):
#set defaults
speed = .1
# make servo position unequal, so we know where the servo
# really is
self._ServoXcp = 99
self._SercoXdp = 100
while True:
time.sleep(speed)
"""
Constantly update self.ServoXcp in case the main process
needs to read it
"""
if self.ServoXcp.empty():
self.ServoXcp.put(self._ServoXcp)
# Constantly read self.ServoXdp in case the main process
# has updated it
if not self.ServoXdp.empty():
self._ServoXdp = self.ServoXdp.get()
# Constantly read self.ServoXs in case the main process
# has updated it with the higher speed value, the
# shorter the wait between loops will be, so the
# servo moves faster
if not self.ServoXs.empty():
_ServoXs = self.ServoXs.get()
speed = .1 / _ServoXs
if self._ServoXcp < self._ServoXdp:
self._ServoXcp += 1 #increment self.ServoXcp
self.ServoXcp.put(self._ServoXcp) # move the servo a little bit
self.servoBlaster.write('0=' + str(self._ServoXcp) + '\n')
self.servoBlaster.flush()
elif self._ServoXcp > self._ServoXdp:
self._ServoXcp -= 1 # decrement self.ServoXcp
# move the servo a little bit
self.ServoXcp.put(self._ServoXcp) # move the servo a little bit
self.servoBlaster.write('0=' + str(self._ServoXcp) + '\n')
self.servoBlaster.flush()
# throw away the old self.ServoXcp value.
if not self.ServoXcp.empty():
trash = self.ServoXcp.get()
elif self._ServoXcp == self._ServoXdp:
_ServoXs = 1 # slow the speed; no need to eat CPU just waiting
# process Y controls servoY
def PY(self):
#set defaults
speed = .1
# make servo position unequal, so we know where the servo
# really is
self._ServoYcp = 99
_SercoYdp = 100
while True:
time.sleep(speed)
"""
Constantly update self.ServoYcp in case the main process
needs to read it
"""
if self.ServoYcp.empty():
self.ServoYcp.put(self._ServoYcp)
# Constantly read self.ServoYdp in case the main process
# has updated it
if not self.ServoYdp.empty():
self._ServoYdp = self.ServoYdp.get()
# Constantly read self.ServoYs in case the main process
# has updated it with the higher speed value, the
# shorter the wait between loops will be, so the
# servo moves faster
if not self.ServoYs.empty():
_ServoYs = self.ServoYs.get()
speed = .1 / _ServoYs
if self._ServoYcp < self._ServoYdp:
self._ServoYcp += 1 #increment self.ServoYcp
self.ServoYcp.put(self._ServoYcp) # move the servo a little bit
self.servoBlaster.write('1=' + str(self._ServoYcp) + '\n')
self.servoBlaster.flush()
elif self._ServoYcp > self._ServoYdp:
self._ServoYcp -= 1 # decrement self.ServoYcp
# move the servo a little bit
self.ServoYcp.put(self._ServoYcp) # move the servo a little bit
self.servoBlaster.write('1=' + str(self._ServoYcp) + '\n')
self.servoBlaster.flush()
# throw away the old self.ServoYcp value.
if not self.ServoYcp.empty():
trash = self.ServoYcp.get()
elif self._ServoYcp == self._ServoYdp:
_ServoYs = 1 # slow the speed; no need to eat CPU just waiting
def run(self):
# start subprocesses
Process(target=self.PX, args=()).start()
Process(target=self.PY, args=()).start()
# wait for them to start
time.sleep(1)
"""
Functions for tuning
"""
def sweep_noSleep(self):
self.pwm.set_pulse_start_in_micros(self.servo_Y.SERVO,0)
for i in range(500, 2501, 3):
self.pwm.set_pulse_length_in_micros(self.servo_Y.SERVO, i)
self.pwm.set_pulse_length_in_micros(self.servo_Y.SERVO, i)
self.pwm.update()
def moveTopRight(self):
self.servo_X.setDegree(0)
self.servo_Y.setDegree(180)
def sweep(self):
i = 0
while True:
try:
if i >= 180:
step = -1
elif i <= 0:
step = 1
self.servo_X.setDegree(i)
self.servo_Y.setDegree(i)
i = i + step
time.sleep(0.01)
self.servo_X.rest()
self.servo_Y.rest()
except KeyboardInterrupt:
break
print("\ntidying up")
self.pi.stop()
def moveX(self,degree):
self.servo_X.setDegree(degree)
if __name__ == "__main__":
import time
import CamControl
print "Running"
cam = CamControl.CamControl()
cam.run()
cam.camRight(50,3)
cam.camUp(50,3)
"""
i = 0
while True:
if i % 2 == 0:
cam.camRight(50,3)
cam.camUp(50,3)
else:
cam.camLeft(50,3)
cam.camDown(50,3)
time.sleep(3)
i += 1
"""
print "stop"