$ vi dual_axis_servo.py
|
<![CDATA[#!/usr/bin/env python
import RPi.GPIO as GPIO
# Set the GPIO pins by physical pin number
GPIO.setmode(GPIO.BOARD)
pan_servo_pin = 16
tilt_servo_pin = 18
# Set the GPIO pins by BCM pin number
# GPIO.setmode(GPIO.BCM)
# pan_servo_pin = 23
# tilt_servo_pin = 24
GPIO.setup(pan_servo_pin, GPIO.OUT)
GPIO.setup(tilt_servo_pin, GPIO.OUT)
# PWM (Pulse Width Modulation) の設定
# SG90 の周波数は50Hz (周期20ms)
pan_servo = GPIO.PWM(pan_servo_pin, 50)
tilt_servo = GPIO.PWM(tilt_servo_pin, 50)
from time import sleep
# 指定角度 (単位 : 度) にサーボを定位
def servo_angle(servo, degree):
duty = 9.5 * degree / 180 + 7.25
if servo == 'pan' :
pan_servo.ChangeDutyCycle(duty)
else:
tilt_servo.ChangeDutyCycle(duty)
sleep(0.3)
# degree1 から degree2 に回転
def servo_rotation(servo, degree1, degree2):
if degree1 < degree2 :
step = 10
else:
step = -10
for degree in range(degree1, degree2, step):
duty = 9.5 * degree / 180 + 7.25
if servo == 'pan' :
pan_servo.ChangeDutyCycle(duty)
else:
tilt_servo.ChangeDutyCycle(duty)
sleep(0.3)
import sys
# exit プロセス
def destroy():
#サーボモータをストップ
servo_angle('pan', 0)
pan_servo.stop()
servo_angle('tilt', 0)
tilt_servo.stop()
#GPIOをクリーンアップ
GPIO.cleanup()
#プログラムを終了
sys.exit()
# pan は 0° (Duty比 7.25) から，
# tilt は -90° (Duty比 2.5) から，それぞれスタート
pan_deg = 0
pan_servo.start(7.25)
sleep(0.3)
tilt_deg = -90
tilt_servo.start(2.5)
sleep(0.3)
# while文で無限ループ
while True:
try:
servo = 'pan'
degree1 = pan_deg
if degree1 == 90:
degree2 = -90
else:
degree2 = degree1 + 30
servo_rotation( servo, degree1, degree2 )
pan_deg = degree2
servo = 'tilt'
degree1 = tilt_deg
if degree1 == 90:
degree2 = -90
else:
degree2 = degree1 + 30
servo_rotation( servo, degree1, degree2 )
tilt_deg = degree2
# Ctrl + C で，プログラム終了
except KeyboardInterrupt:
destroy()]]>
|
<![CDATA[$ chmod +x dual_axis_servo.py
$ ./dual_axis_servo.py]]>
|