-
Notifications
You must be signed in to change notification settings - Fork 66
/
servo_alt.py
148 lines (104 loc) · 3.4 KB
/
servo_alt.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
"""
File: chapter10/servo_alt.py
Controlling a servo.
Dependencies:
pip3 install pigpio
Built and tested with Python 3.7 on Raspberry Pi 4 Model B
"""
from time import sleep
import pigpio
SERVO_GPIO = 21
# Pulse widths for extreme left/right and center positions in nanoseconds.
# The default values are 'typical' values for a hobby servo.
# Be gradual when changing the left and right adjustments
# because a servo can be damaged if rotated beyond its limits.
RIGHT_PULSE = 1000 # Smaller values for 'more' right
LEFT_PULSE = 2000 # Higher values for 'more' left
# CENTER_PULSE = 1500 # or calculate as below
CENTER_PULSE = ((LEFT_PULSE - RIGHT_PULSE) // 2) + RIGHT_PULSE
# Delay to give servo time to move
MOVEMENT_DELAY_SECS = 0.5
pi = pigpio.pi()
# Servos commonly operate at 50Hz, that is one pulse every 20ms (1 second / 50 Hz = 0.02)
pi.set_PWM_frequency(SERVO_GPIO, 50)
PULSE_WIDTH = 20000.0 # 20000 microsecond = 20 milliseconds = 0.02 seconds
DUTYCYCLE_RANGE = 100
# DUTYCYCLE_RANGE = pi.get_PWM_range(SERVO_GPIO) # Default is 255
pi.set_PWM_range(SERVO_GPIO, DUTYCYCLE_RANGE)
def idle():
"""
Idle servo (zero pulse width).
Servo will be rotatable by hand with little force.
"""
# WAS
# pi.set_servo_pulsewidth(SERVO_GPIO, 0)
# NOW
dutycycle = 0
pi.set_PWM_dutycycle(SERVO_GPIO, dutycycle)
def center():
"""
Center the servo.
"""
# WAS
# pi.set_servo_pulsewidth(SERVO_GPIO, CENTER_PULSE)
# NOW
dutycycle_percent = CENTER_PULSE / PULSE_WIDTH
# Scale duty cycle percentage into PiGPIO duty cycle range
dutycycle = dutycycle_percent * DUTYCYCLE_RANGE
pi.set_PWM_dutycycle(SERVO_GPIO, dutycycle)
def left():
"""
Rotate servo to full left position.
"""
# WAS
# pi.set_servo_pulsewidth(SERVO_GPIO, LEFT_PULSE)
# NOW
dutycycle_percent = LEFT_PULSE / PULSE_WIDTH
# Scale duty cycle percentage into PiGPIO duty cycle range
dutycycle = dutycycle_percent * DUTYCYCLE_RANGE
pi.set_PWM_dutycycle(SERVO_GPIO, dutycycle)
def right():
"""
Rotate servo to full right position.
"""
# WAS
# pi.set_servo_pulsewidth(SERVO_GPIO, RIGHT_PULSE)
# NOWz
dutycycle_percent = RIGHT_PULSE / PULSE_WIDTH
# Scale duty cycle percentage into PiGPIO duty cycle range
dutycycle = dutycycle_percent * DUTYCYCLE_RANGE
pi.set_PWM_dutycycle(SERVO_GPIO, dutycycle)
def angle(to_angle):
"""
Rotate servo to specified angle (between -90 and +90 degrees)
"""
# Restrict to -90..+90 degrees
to_angle = int(min(max(to_angle, -90), 90))
ratio = (to_angle + 90) / 180.0
pulse_range = LEFT_PULSE - RIGHT_PULSE
pulse = LEFT_PULSE - round(ratio * pulse_range)
# WAS
# pi.set_servo_pulsewidth(SERVO_GPIO, pulse)
# NOW
# Pulse in seconds divided by frequency in Hertz
dutycycle_percent = (pulse / 1000) / 50
# Scale duty cycle percentage into PiGPIO duty cycle range
dutycycle = dutycycle_percent * DUTYCYCLE_RANGE
pi.set_PWM_dutycycle(SERVO_GPIO, dutycycle)
def sweep(count=4):
"""
Sweep servo horn left and right 'count' times.
"""
left() # Starting position
sleep(MOVEMENT_DELAY_SECS)
for i in range(count):
right()
sleep(MOVEMENT_DELAY_SECS)
left()
sleep(MOVEMENT_DELAY_SECS)
if __name__ == '__main__':
try:
sweep()
finally:
idle()
pi.stop() # PiGPIO Cleanup