-
Notifications
You must be signed in to change notification settings - Fork 0
/
servo_test.py
149 lines (136 loc) · 4.2 KB
/
servo_test.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
# Isolated test module for servo control using Adafruit servo control board
import servoControl
from time import sleep
# Instantiate object
servo = servoControl.ServoControl()
# Minimum and maximum allowable servo range
_MIN = 50
_MAX = 900
# Verify degrees are valid and within bounds
def degreeVerify():
flag = 'Invalid'
while flag == 'Invalid':
degree = input("Degree: ")
if degree.isnumeric():
degree = int(degree)
if degree > _MIN and degree < _MAX:
return degree
else:
print("Invalid. Degree out of range")
else:
print("Invalid. Not numeric input")
try:
# Initially center servos
servo.panCenter()
#servo.tiltCenter()
# Ensure valid mode selection
mode = input('''
default servo mode - 1
exact servo mode - 2
''')
if mode.isdigit():
mode = int(mode)
while mode != 1 and mode != 2:
mode = input('''
default servo mode - 1
exact servo mode - 2
''')
if mode.isdigit():
mode = int(mode)
# Default servo mode
if mode == 1:
print('-' * 60)
print("Default servo mode")
print('-' * 60)
while True:
print('''
Look left l
Look right r
Look center c
Mode m
Reset re
Quit q
''')
command = str(input("Enter command: ")).lower()
if command == 'l':
print("Left")
servo.panLeft()
elif command == 'r':
print("Right")
servo.panRight()
elif command == 'c':
print("Center")
servo.panCenter()
#elif command == 'tu':
# print("Tilt up")
# servo.tiltUp()
#elif command == 'td':
# print("Tilt down")
# servo.tiltDown()
#elif command == 'tc':
# print("Tilt center")
# servo.tiltCenter()
elif command == 're':
print("Reset")
servo.tiltCenter()
servo.panCenter()
elif command =='m':
print('-' * 60)
print("Default servo mode")
print('-' * 60)
elif command == 'q':
# Reset servo to center
servo.resetServo()
print("Quit")
exit(1)
# Exact servo mode
else:
print('-' * 60)
print("Exact servo mode")
print('-' * 60)
while True:
print('''
Look left l
Look right r
Look center c
Mode m
Reset re
Quit q
''')
command = str(input("Enter command: ")).lower()
if command == 'l':
print("Left")
servo.panExactLeft(degreeVerify())
elif command == 'r':
print("Right")
servo.panExactRight(degreeVerify())
elif command == 'c':
print("Center")
servo.panExactCenter(degreeVerify())
#elif command == 'tu':
# print("Tilt up")
# servo.tiltExactUp(degreeVerify())
#elif command == 'td':
# print("Tilt down")
# servo.tiltExactDown(degreeVerify())
#elif command == 'tc':
# print("Tilt center")
# servo.tiltExactCenter(degreeVerify())
elif command == 're':
print("Reset")
servo.panCenter()
servo.tiltCenter()
elif command =='m':
print('-' * 60)
print("Exact servo mode")
print('-' * 60)
elif command == 'q':
# Reset servo to center
servo.resetServo()
print("Quit")
exit(1)
except KeyboardInterrupt:
# Reset servo to center
servo.resetServo()
print("Quit")
exit(1)