#-*- coding: UTF-8 -*-
import RPi.GPIO as GPIO
import time
def angle_to_duty_cycle(angle=0):
duty_cycle = (0.05 * SERVO_MOTOR_FREQUENCY) + (0.19 * SERVO_MOTOR_FREQUENCY * angle / SERVO_MOTOR_ANGLE_MAX)
return duty_cycle
def move_servo(pin, pwm, angle):
dc = angle_to_duty_cycle(angle)
pwm.ChangeDutyCycle(dc)
print('第{: >2}軸 角度={: >3}, 工作週期={:.2f}'.format(pin, angle, dc))
def manual_control(pin, pwm):
print('Move the servo manually (press "x" to exit).')
input_data = None
while True:
input_data = input("Enter the angle: ")
if input_data.lower() == 'x':
break
try:
angle = int(input_data)
except Exception:
print('Either enter an integer or "x" to exit.')
continue
try:
move_servo(pin, pwm, angle)
except ValueError:
print('The inputted angle is not valid.')
continue
def automatic_control(pin, pwm):
print('Move the servo with a test pattern.')
angle=SERVO_MOTOR_ANGLE_MIN
move_servo(pin, pwm, angle)
time.sleep(3)
angle=SERVO_MOTOR_ANGLE_MAX
move_servo(pin, pwm, angle)
time.sleep(3)
angle=90
move_servo(pin, pwm, angle)
time.sleep(3)
angle=110
move_servo(pin, pwm, angle)
time.sleep(3)
try:
CONTROL_PIN1 = 4
CONTROL_PIN2 = 17
CONTROL_PIN3 = 22
CONTROL_PIN4 = 23
CONTROL_PIN5 = 24
CONTROL_PIN6 = 27
SERVO_MOTOR_ANGLE_MIN = 0
SERVO_MOTOR_ANGLE_MAX = 270
SERVO_MOTOR_FREQUENCY = 50
GPIO.setmode(GPIO.BCM)
GPIO.setup(CONTROL_PIN1, GPIO.OUT)
GPIO.setup(CONTROL_PIN2, GPIO.OUT)
GPIO.setup(CONTROL_PIN3, GPIO.OUT)
GPIO.setup(CONTROL_PIN4, GPIO.OUT)
GPIO.setup(CONTROL_PIN5, GPIO.OUT)
GPIO.setup(CONTROL_PIN6, GPIO.OUT)
pwmList=list()
pwmList.append(GPIO.PWM(CONTROL_PIN1, SERVO_MOTOR_FREQUENCY))
pwmList.append(GPIO.PWM(CONTROL_PIN2, SERVO_MOTOR_FREQUENCY))
pwmList.append(GPIO.PWM(CONTROL_PIN3, SERVO_MOTOR_FREQUENCY))
pwmList.append(GPIO.PWM(CONTROL_PIN4, SERVO_MOTOR_FREQUENCY))
pwmList.append(GPIO.PWM(CONTROL_PIN5, SERVO_MOTOR_FREQUENCY))
pwmList.append(GPIO.PWM(CONTROL_PIN6, SERVO_MOTOR_FREQUENCY))
for pwm in pwmList:
pwm.start(0)
#pwm.ChangeDutyCycle(0)
move_servo(1, pwmList[1], 50)
#time.sleep(3)
move_servo(2, pwmList[2], 45)
#time.sleep(2)
move_servo(3, pwmList[3], 90)
#time.sleep(2)
move_servo(0, pwmList[0], 75)
#time.sleep(2)
while True:
input_data = input("What servo motor (channel) you want to control? ")
try:
servo_aix = int(input_data)
if servo_aix < 0 or servo_aix > 5:
raise ValueError
except Exception:
print('Enter an integer between 0 and 5.')
continue
input_data = str(input("What do you want to do?\n[1] Manual test\n[2] Automatic test\n[x] Exit\nAnswer: "))
if input_data == '1':
manual_control(servo_aix, pwmList[servo_aix])
elif input_data == '2':
automatic_control(servo_aix, pwmList[servo_aix])
elif input_data.lower() == 'x':
break
else:
print('Unrecognized input.')
continue
except KeyboardInterrupt:
print('關閉程式')
finally:
for pwm in pwmList:
pwm.stop(0)
GPIO.cleanup()