ラズパイで３つのサーボを動かしてみる

#はじめに
ベゼリーという小型ロボット開発キット向けにラズパイからPythonで３個のサーボを制御するプログラムを書いたのですが、ベゼリー以外の用途でも応用できると思いますので、こちらで公開させていただきます。
#対象者

• ラズパイでPython使って３個以上のサーボを動かしたいかた。

#特徴

• ラズパイで３個のRCサーボを制御します。
• サーボのセンタリング位置を０度として、プラスマイナス９０度の角度指定でサーボを動かせます。
• サーボごとに回転の限界角度を設定できます。
• 回転速度を指定できます。

#準備

• ラズパイ。I2Cを有効にしておいてください。
• PCA9685を搭載したサーボドライバーボード
  • PCA9685はNXPのPWMコントローラです。
  • これを搭載したサーボドライバーボードがAdafruitなど各社から発売されています。
  • ラズパイとはI2Cで接続します。
• 小型サーボSG90 digital　✕３個
  • Tower Proの安価なRCサーボです。
• ラズパイとサーボドライバボードとサーボとサーボ用電源を接続してください。
  • こちらのページが参考になるかと。

#コード

bezelie.py

# -*- coding: utf-8 -*-
# Bezelie Python Module for Raspberry Pi
import RPi.GPIO as GPIO
from time import sleep
import smbus
import math

bus = smbus.SMBus(1)
address_pca9685 = 0x40 # If you connect other I2C devices, you might change thi$

# Constants
dutyMax = 490     #
dutyMin = 110     #
dutyCenter = 300  #
steps = 1         #

# Global Valiables
headNow = backNow = stageNow = dutyCenter

# Definitions
def initPCA9685():
  bus.write_byte_data(address_pca9685, 0x00, 0x00)
  freq = 0.9*50
  prescaleval = 25000000.0    # 25MHz
  prescaleval /= 4096.0       # 12-bit
  prescaleval /= float(freq)
  prescaleval -= 1.0
  prescale = int(math.floor(prescaleval + 0.5))
  oldmode = bus.read_byte_data(address_pca9685, 0x00)
  newmode = (oldmode & 0x7F) | 0x10
  bus.write_byte_data(address_pca9685, 0x00, newmode)
  bus.write_byte_data(address_pca9685, 0xFE, prescale)
  bus.write_byte_data(address_pca9685, 0x00, oldmode)
  sleep(0.005)
  bus.write_byte_data(address_pca9685, 0x00, oldmode | 0xa1)

def setPCA9685Duty(channel, on, off):
  channelpos = 0x6 + 4*channel
  try:
    bus.write_i2c_block_data(address_pca9685, channelpos, [on&0xFF, on>>8, off&$
  except IOError:
    pass

def moveServo (id, degree, adj, max, min, speed, now):
  dst = (dutyMin-dutyMax)*(degree+adj+90)/180 + dutyMax
  if speed == 0:
    setPCA9685Duty(id, 0, dst)
    sleep(0.001 * math.fabs(dst-now))
    now = dst
  if dst > max: dst = max
  if dst < min: dst = min
  while (now != dst):
    if now < dst:
      now += steps
      if now > dst: now = dst
    else:
      now -= steps
      if now < dst: now = dst
    setPCA9685Duty(id, 0, now)
    sleep(0.004 * steps *(speed))
  return (now)

def moveHead (degree, speed=1):
  adj = 0       # Head servo adjustment
  max = 490     # Downward limit
  min = 110     # Upward limit
  global headNow
  headNow = moveServo (2, degree, adj, max, min, speed, headNow)

def moveBack (degree, speed=1):
  adj = 0       # Back servo adjustment
  max = 490     # AntiClockwise limit
  min = 110     # Clockwise limit
  global backNow
  backNow = moveServo (1, degree, adj, max, min, speed, backNow)

def moveStage (degree, speed=1):
  adj = 0      # Stage servo adjustment
  max = 490    # AntiClockWise limit
  min = 110    # Clocwise limit
  global stageNow
  stageNow = moveServo (0, degree, adj, max, min, speed,stageNow)

#解説
-　３つのサーボには便宜的に、Head, Back, Stageという名前をつけており、moveHead(), moveBack(), moveStage()がそれぞれを動かす命令になっています。
-　引数degreeにはプラスマイナス９０度の範囲で角度を入れます。０がセンター位置。正の値を入れると時計回りに回ります。
-　引数speedは省略すると１になり、大きな値を入れるほどゆっくり動きます。０を入れると最速で動きますが、実行後に適切な長さのtime.sleep()を入れてあげないとプロセスが溜まって挙動がおかしくなってしまいます。
-　サーボSG90は１８０度回転することができますが、ロボットなどの構造上、回転角度に制限をもたせたい場合もあるでしょう。そんなときはサーボごとにmaxとminを調整してください。
-　サーボのセンタリング位置を微調整したい場合は、adjの値を変えてみてください。

#サンプル

• 上記bezelie.pyを含むサンプルコードはgithubで公開しております。
• サンプルプログラムの使いかたについては、ベゼリー公式ページをご参照ください。

#謝辞
プログラムの作成にあたっては、金丸隆志さんの著書「RaspberryPiで学ぶ電子工作」と、Adafruit社のライブラリを参考にさせていただきました。お礼を申し上げます。