-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathPCAde9685 -support for more servos.py
573 lines (490 loc) · 20.8 KB
/
PCAde9685 -support for more servos.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
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
import time
import sys
import random
# Import the PCA9685 module.
import Adafruit_PCA9685
# Initialise the PCA9685 using the default address (0x40).
pwm = Adafruit_PCA9685.PCA9685()
# Alternatively specify a different address and/or bus:
# pwm = Adafruit_PCA9685.PCA9685(address=0x41, busnum=2)
# Configure min and max servo pulse lengths
servo_min = 120
servo_max = 650
servo_frequency = 60
# Set frequency to 60hz, good for servos.
pwm.set_pwm_freq(servo_frequency)
def translate(value, left_min, left_max, right_min, right_max):
# Figure out how 'wide' each range is
left_span = left_max - left_min
right_span = right_max - right_min
# Convert the left range into a 0-1 range (float)
value_scaled = float(value - left_min) / float(left_span)
# Convert the 0-1 range into a value in the right range.
return right_min + (value_scaled * right_span)
class Servo:
# frequency is the number of pulses per second
# each pulse has 4096 clock sections
# on: The tick (between 0 and 4095) when the signal should transition from low to high
# off:the tick (between 0 and 4095) when the signal should transition from high to low
def __init__(self, channel, servo_min_bound=0, servo_max_bound=180, current_angle="unknown", info_print=False):
self.channel = channel
self.currentAngle = current_angle
self.info_print = info_print
self.servo_min_bound = servo_min_bound
self.servo_max_bound = servo_max_bound
if self.info_print:
print(
f'Initializing Servo on channel {self.channel} with range {self.servo_min_bound} '
f'to {self.servo_max_bound} degrees.')
def __str__(self):
return f"Servo on channel {self.channel} at {self.currentAngle} degrees"
def set_info_print(self, info_print):
self.info_print = info_print
def set_angle(self, angle=90, delay_amount=0.3, clock_start=0):
self.currentAngle = angle
if self.servo_min_bound != 0 or self.servo_max_bound != 180:
angle = translate(angle, 0, 180, self.servo_min_bound, self.servo_max_bound)
self.currentAngle = angle
duty_cycle = angle / 180
pulse_width = 548 * duty_cycle + 120
pulse_width = int(pulse_width)
if 0 <= clock_start <= 4095:
pwm.set_pwm(self.channel, clock_start, pulse_width + clock_start)
# pwm.set_pwm(channel, on , off) #on and off are 12-bit values so they are in between 0 and 4095
# pwm.set_pwm_freq(freq) in hz
else:
print("Invalid clock start time, resetting to zero...")
pwm.set_pwm(self.channel, 0, pulse_width)
if self.info_print:
print(
f"Setting Servo on channel {self.channel} to {self.currentAngle} "
f"on clock starting time {clock_start} and waiting {delay_amount} seconds")
time.sleep(delay_amount)
def glide_angle(self, starting_angle, ending_angle, time_to_take):
if self.info_print:
print(
f"Servo on channel {self.channel} gliding from angle {starting_angle} to {ending_angle} "
f"in {time_to_take} seconds")
self.info_print = False
gliding_info_print = True
else:
gliding_info_print = False
time_interval = time_to_take / abs((starting_angle - ending_angle))
self.set_angle(starting_angle, 0.5)
if starting_angle < ending_angle:
for angle_i in range(starting_angle + 1, ending_angle + 1):
self.set_angle(angle_i, time_interval)
else:
neg_angle = starting_angle - 1
while neg_angle != ending_angle:
neg_angle -= 1
self.set_angle(neg_angle, time_interval)
if gliding_info_print:
self.info_print = True
def random_angle(self, random_time=200):
random_time = (random.randint(0, random_time)) / 1000.0
self.set_angle(random.randint(0, 180), random_time)
def vibrate(self, start_at=0, interval=15, delay_amount=3, duration=100):
print("vibrate starting...")
start_time = time.time() * 1000
end_time = start_time
duration_millis = duration * 1000
for i in range(start_at, 180):
self.set_angle(0, 0)
time.sleep(delay_amount)
self.set_angle(i, 0)
time.sleep(delay_amount)
i += interval
if (end_time - start_time) <= duration_millis:
end_time = time.time() * 1000
else:
break
self.set_angle(0, 0)
print("vibrate ending...")
class ServoGroup:
# frequency is the number of pulses per second
# each pulse has 4096 clock sections
# on: The tick (between 0 and 4095) when the signal should transition from low to high
# off:the tick (between 0 and 4095) when the signal should transition from high to low
def __init__(self, num_of_servos, *channels, current_angle="unknown", info_print=False):
self.currentAngle = current_angle
self.info_print = info_print
self.num_of_servos = num_of_servos
self.channels = channels # tuple of channels
if len(self.channels) != num_of_servos:
sys.exit("\n\nDid not give enough channels for servos!\n")
def __str__(self):
return f":self.num_of_servos servos on channels :self.channels at :self.currentAngle degrees"
def set_info_print(self, info_print):
self.info_print = info_print
def set_angle(self, angle=90, delay_amount=1, clock_start=0):
self.currentAngle = angle
duty_cycle = angle / 180
pulse_width = 548 * duty_cycle + 120
pulse_width = int(pulse_width)
if 0 <= clock_start <= 4095:
for channel in self.channels:
pwm.set_pwm(channel, clock_start, pulse_width + clock_start)
# pwm.set_pwm(channel, on , off) #on and off are 12-bit values so they are in between 0 and 4095
# pwm.set_pwm_freq(freq) in hz
else:
for channel in self.channels:
print("Invalid clock start time, resetting to zero...")
pwm.set_pwm(channel, 0, pulse_width)
if self.info_print:
print(
f"Setting servos on channels {self.channels} to {self.currentAngle} "
f"on clock starting time {clock_start}")
time.sleep(delay_amount)
class ServoGroup2:
def __init__(self, list_of_servos, info_print=False):
self.info_print = info_print
self.list_of_servos = list_of_servos
if self.info_print:
print(f"Initiating Servo Group with {len(list_of_servos)} members")
def set_angle(self, angle=90, delay_amount=1.0, clock_start=0):
info_print_list = []
if self.info_print:
print(f"Setting Servo Group to {angle} and waiting {delay_amount} seconds")
for i in range(0, len(self.list_of_servos)):
if self.list_of_servos[i].info_print:
self.list_of_servos[i].set_info_print(False)
info_print_list.append(i)
else:
info_print_list.append(-1)
self.list_of_servos[i].set_angle(angle, 0, clock_start)
if self.info_print:
for i in range(0, len(self.list_of_servos)):
if i == info_print_list[i]:
self.list_of_servos[i].set_info_print(True)
time.sleep(delay_amount)
def glide_angle(self, starting_angle, ending_angle, time_to_take):
if self.info_print:
print(
f"ServoGroup gliding from angle {starting_angle} to {ending_angle} in {time_to_take} seconds")
self.info_print = False
gliding_info_print = True
else:
gliding_info_print = False
time_interval = time_to_take / abs((starting_angle - ending_angle))
self.set_angle(starting_angle, 0.5)
if starting_angle < ending_angle:
for angle_i in range(starting_angle + 1, ending_angle + 1):
self.set_angle(angle_i, time_interval)
else:
neg_angle = starting_angle - 1
while neg_angle != ending_angle:
neg_angle -= 1
self.set_angle(neg_angle, time_interval)
if gliding_info_print:
self.info_print = True
class ServoPumpkin:
def __init__(self, *eyes):
self.eyes = eyes
def reset_out(self, delay_amount=2):
self.eyes[0].set_angle(180, 0)
self.eyes[1].set_angle(180, 0)
self.eyes[2].set_angle(180, 0)
self.eyes[3].set_angle(0, 0)
self.eyes[4].set_angle(180, 0)
self.eyes[5].set_angle(0, 0)
self.eyes[6].set_angle(0, 0)
self.eyes[7].set_angle(0, 0)
#Could add more eyes by adding more lines and angles (do this for each routine) for example add a 9th servo - self.eyes[8].set_angle(0,0)
time.sleep(delay_amount)
def random_eyes(self, duration, random_time=200): # give duration of running in seconds
print("pumpkin random starting...")
start_time = time.time() * 1000
end_time = start_time
duration_millis = duration * 1000
while (end_time - start_time) <= duration_millis:
for eye in self.eyes:
eye.random_angle(random_time)
end_time = time.time() * 1000
print("pumpkin random ending...")
def min_max(self, duration, delay_amount=1): # give duration of running in seconds
print("pumpkin min_max starting...")
start_time = time.time() * 1000
end_time = start_time
duration_millis = duration * 1000
while (end_time - start_time) <= duration_millis:
time.sleep(delay_amount)
self.eyes[0].set_angle(0, 0)
self.eyes[1].set_angle(0, 0)
self.eyes[2].set_angle(0, 0)
self.eyes[3].set_angle(180, 0)
self.eyes[4].set_angle(0, 0)
self.eyes[5].set_angle(180, 0)
self.eyes[6].set_angle(180, 0)
self.eyes[7].set_angle(180, 0)
#Could add more eyes by adding more lines and angles- self.eyes[8].set_angle(0,0)
time.sleep(delay_amount)
self.eyes[0].set_angle(180, 0)
self.eyes[1].set_angle(180, 0)
self.eyes[2].set_angle(180, 0)
self.eyes[3].set_angle(0, 0)
self.eyes[4].set_angle(180, 0)
self.eyes[5].set_angle(0, 0)
self.eyes[6].set_angle(0, 0)
self.eyes[7].set_angle(0, 0)
#Could add more eyes by adding more lines and angles- self.eyes[8].set_angle(0,0)
end_time = time.time() * 1000
print("pumpkin min_max ending...")
def min_max_glide(self, eye_speed, delay_amount=0.5):
self.reset_out(4)
print("pumpkin min_max_glide starting...")
if eye_speed >= 0.3:
self.eyes[0].glide_angle(180, 0, eye_speed)
self.eyes[1].glide_angle(180, 0, eye_speed)
self.eyes[2].glide_angle(180, 0, eye_speed)
self.eyes[3].glide_angle(0, 180, eye_speed)
self.eyes[4].glide_angle(0, 180, eye_speed)
self.eyes[5].glide_angle(0, 180, eye_speed)
self.eyes[6].glide_angle(0, 180, eye_speed)
self.eyes[7].glide_angle(180, 0, eye_speed)
time.sleep(delay_amount)
self.eyes[4].glide_angle(0, 180, eye_speed)
self.eyes[5].glide_angle(180, 0, eye_speed)
self.eyes[6].glide_angle(180, 0, eye_speed)
self.eyes[7].glide_angle(180, 0, eye_speed)
self.eyes[3].glide_angle(180, 0, eye_speed)
self.eyes[2].glide_angle(0, 180, eye_speed)
self.eyes[1].glide_angle(0, 180, eye_speed)
self.eyes[0].glide_angle(0, 180, eye_speed)
else:
self.eyes[0].set_angle(0, eye_speed)
self.eyes[1].set_angle(0, eye_speed)
self.eyes[2].set_angle(0, eye_speed)
self.eyes[3].set_angle(180, eye_speed)
self.eyes[7].set_angle(180, eye_speed)
self.eyes[6].set_angle(180, eye_speed)
self.eyes[5].set_angle(180, eye_speed)
self.eyes[4].set_angle(0, eye_speed)
time.sleep(delay_amount)
self.eyes[4].set_angle(180, eye_speed)
self.eyes[5].set_angle(0, eye_speed)
self.eyes[6].set_angle(0, eye_speed)
self.eyes[7].set_angle(0, eye_speed)
self.eyes[3].set_angle(0, eye_speed)
self.eyes[2].set_angle(180, eye_speed)
self.eyes[1].set_angle(180, eye_speed)
self.eyes[0].set_angle(180, eye_speed)
print("pumpkin min_max_glide ending...")
def half_half(self, delay_amount=1):
self.reset_out()
print("Half_Half starting...")
# start half half
self.eyes[0].set_angle(0, 0)
self.eyes[1].set_angle(0, 0)
self.eyes[4].set_angle(0, 0)
self.eyes[5].set_angle(180, 0)
time.sleep(delay_amount)
self.eyes[0].set_angle(180, 0)
self.eyes[1].set_angle(180, 0)
self.eyes[4].set_angle(180, 0)
self.eyes[5].set_angle(0, 0)
time.sleep(delay_amount)
self.eyes[2].set_angle(0, 0)
self.eyes[3].set_angle(180, 0)
self.eyes[6].set_angle(180, 0)
self.eyes[7].set_angle(180, 0)
time.sleep(delay_amount)
self.eyes[2].set_angle(180, 0)
self.eyes[3].set_angle(0, 0)
self.eyes[6].set_angle(0, 0)
self.eyes[7].set_angle(0, 0)
print("Half_Half ending...")
def columns(self, delay_amount=1):
self.reset_out()
print("columns starting...")
# start each column is a group
# column 1:
self.eyes[0].set_angle(0, 0)
self.eyes[4].set_angle(0, 0)
time.sleep(delay_amount)
self.eyes[0].set_angle(180, 0)
self.eyes[4].set_angle(180, 0)
time.sleep(delay_amount)
# column 2:
self.eyes[1].set_angle(0, 0)
self.eyes[5].set_angle(180, 0)
time.sleep(delay_amount)
self.eyes[1].set_angle(180, 0)
self.eyes[5].set_angle(0, 0)
time.sleep(delay_amount)
# column 3:
self.eyes[2].set_angle(0, 0)
self.eyes[6].set_angle(180, 0)
time.sleep(delay_amount)
self.eyes[2].set_angle(180, 0)
self.eyes[6].set_angle(0, 0)
time.sleep(delay_amount)
# column 4:
self.eyes[3].set_angle(180, 0)
self.eyes[7].set_angle(180, 0)
time.sleep(delay_amount)
self.eyes[3].set_angle(0, 0)
self.eyes[7].set_angle(0, 0)
time.sleep(delay_amount)
print("columns ending...")
def columns_converging(self, delay_amount=1):
self.reset_out()
print("columns converging starting...")
# start each column is a group
# column 1:
self.eyes[0].set_angle(0, 0)
self.eyes[4].set_angle(0, 0)
# column 4:
self.eyes[3].set_angle(180, 0)
self.eyes[7].set_angle(180, 0)
time.sleep(delay_amount)
self.eyes[0].set_angle(180, 0)
self.eyes[4].set_angle(180, 0)
self.eyes[3].set_angle(0, 0)
self.eyes[7].set_angle(0, 0)
time.sleep(delay_amount)
# column 2:
self.eyes[1].set_angle(0, 0)
self.eyes[5].set_angle(180, 0)
# column 3:
self.eyes[2].set_angle(0, 0)
self.eyes[6].set_angle(180, 0)
time.sleep(delay_amount)
self.eyes[1].set_angle(180, 0)
self.eyes[5].set_angle(0, 0)
self.eyes[2].set_angle(180, 0)
self.eyes[6].set_angle(0, 0)
time.sleep(delay_amount)
print("columns converging ending...")
def rows(self, delay_amount=1):
self.reset_out()
print("rows starting...")
# start each row is a group
# top row:
self.eyes[0].set_angle(0, 0)
self.eyes[1].set_angle(0, 0)
self.eyes[2].set_angle(0, 0)
self.eyes[3].set_angle(180, 0)
time.sleep(delay_amount)
self.eyes[0].set_angle(180, 0)
self.eyes[1].set_angle(180, 0)
self.eyes[2].set_angle(180, 0)
self.eyes[3].set_angle(0, 0)
time.sleep(delay_amount)
# bottom row:
self.eyes[4].set_angle(0, 0)
self.eyes[5].set_angle(180, 0)
self.eyes[6].set_angle(180, 0)
self.eyes[7].set_angle(180, 0)
time.sleep(delay_amount)
self.eyes[4].set_angle(180, 0)
self.eyes[5].set_angle(0, 0)
self.eyes[6].set_angle(0, 0)
self.eyes[7].set_angle(0, 0)
time.sleep(delay_amount)
print("rows ending...")
def look_directions(self, delay_amount=1):
self.reset_out()
print("look_directions starting...")
# start bottom halves at same time
self.eyes[4].set_angle(0, 0) # inward
self.eyes[5].set_angle(0, 0) # inward
self.eyes[6].set_angle(0, 0) # outward
self.eyes[7].set_angle(0, 0) # outward
time.sleep(delay_amount)
self.eyes[4].set_angle(180, 0) # outward
self.eyes[5].set_angle(180, 0) # outward
self.eyes[6].set_angle(180, 0) # inward
self.eyes[7].set_angle(180, 0) # inward
time.sleep(delay_amount)
# start top halves at same time
self.eyes[0].set_angle(0, 0) # inward
self.eyes[1].set_angle(0, 0) # inward
self.eyes[2].set_angle(180, 0) # outward
self.eyes[3].set_angle(180, 0) # outward
time.sleep(delay_amount)
self.eyes[0].set_angle(180, 0) # outward
self.eyes[1].set_angle(180, 0) # outward
self.eyes[2].set_angle(0, 0) # inward
self.eyes[3].set_angle(0, 0) # inward
time.sleep(delay_amount)
print("look_directions ending...")
def ladders(self, start_at, interval=15, delay_amount=3.0, duration=100):
print("ladders starting...")
start_time = time.time() * 1000
end_time = start_time
duration_millis = duration * 1000
i = start_at
while i <= 180:
self.eyes[0].set_angle(0, 0)
self.eyes[1].set_angle(0, 0)
self.eyes[2].set_angle(0, 0)
self.eyes[3].set_angle(180, 0)
self.eyes[4].set_angle(0, 0)
self.eyes[5].set_angle(0, 0)
self.eyes[6].set_angle(0, 0)
self.eyes[7].set_angle(0, 0)
time.sleep(delay_amount)
self.eyes[0].set_angle(i, 0)
self.eyes[1].set_angle(i, 0)
self.eyes[2].set_angle(i, 0)
self.eyes[3].set_angle(abs(i - 180), 0)
self.eyes[4].set_angle(i, 0)
self.eyes[5].set_angle(i, 0)
self.eyes[6].set_angle(i, 0)
self.eyes[7].set_angle(i, 0)
time.sleep(delay_amount)
i += interval
if (end_time - start_time) <= duration_millis:
end_time = time.time() * 1000
else:
break
self.eyes[0].set_angle(0, 0)
self.eyes[1].set_angle(0, 0)
self.eyes[2].set_angle(0, 0)
self.eyes[3].set_angle(180, 0)
self.eyes[4].set_angle(0, 0)
self.eyes[5].set_angle(0, 0)
self.eyes[6].set_angle(0, 0)
self.eyes[7].set_angle(0, 0)
print("ladders ending...")
def vibrate_rounds(self):
self.eyes[0].vibrate(30, 1, 0.05, 1)
self.eyes[1].vibrate(30, 1, 0.05, 1)
self.eyes[2].vibrate(30, 1, 0.05, 1)
self.eyes[3].vibrate(30, 1, 0.05, 1)
self.eyes[4].vibrate(30, 1, 0.05, 1)
self.eyes[5].vibrate(30, 1, 0.05, 1)
self.eyes[6].vibrate(30, 1, 0.05, 1)
self.eyes[7].vibrate(30, 1, 0.05, 1)
if __name__ == "__main__":
top_left = Servo(0, 35, 82)
top_mid_left = Servo(1, 137, 180)
top_mid_right = Servo(2, 35, 84)
top_right = Servo(3, 80, 130)
bottom_left = Servo(4, 30, 70)
bottom_mid_left = Servo(5, 90, 140)
bottom_mid_right = Servo(6, 80, 150)
bottom_right = Servo(7, 38, 83)
#add more servos with channels and bound values if needed, then pass into the ServoPumpkin function
pumpkin = ServoPumpkin(top_left, top_mid_left, top_mid_right, top_right,
bottom_left, bottom_mid_left, bottom_mid_right, bottom_right)
eyes = ServoGroup2(
[top_left, top_mid_left, top_mid_right, top_right, bottom_left, bottom_mid_left, bottom_mid_right,
bottom_right])
while True:
pumpkin.vibrate_rounds()
pumpkin.random_eyes(15)
pumpkin.ladders(30, 1, 0.05, 3) # vibrate all for 1 second
pumpkin.min_max(6, 1)
pumpkin.min_max_glide(0.3, 1) # slow glide
pumpkin.rows()
pumpkin.half_half()
pumpkin.min_max_glide(0.2, 0.8) # fast glide
pumpkin.columns_converging()
pumpkin.look_directions()
pumpkin.ladders(0, 10, 0.25) # slow ladders
pumpkin.columns()
eyes.set_angle(0)
eyes.glide_angle(0, 180, 1.5)
pumpkin.ladders(0, 2, 0.1) # fast ladders