Python
RaspberryPi
Bezelie
PCA9685
SG90

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

More than 1 year has passed since last update.


はじめに

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


対象者


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


特徴


  • ラズパイで3個のRCサーボを制御します。

  • サーボのセンタリング位置を0度として、プラスマイナス90度の角度指定でサーボを動かせます。

  • サーボごとに回転の限界角度を設定できます。

  • 回転速度を指定できます。


準備


  • ラズパイ。I2Cを有効にしておいてください。

  • PCA9685を搭載したサーボドライバーボード


    • PCA9685はNXPのPWMコントローラです。

    • これを搭載したサーボドライバーボードがAdafruitなど各社から発売されています。

    • ラズパイとはI2Cで接続します。



  • 小型サーボSG90 digital ✕3個


    • 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)



解説

- 3つのサーボには便宜的に、Head, Back, Stageという名前をつけており、moveHead(), moveBack(), moveStage()がそれぞれを動かす命令になっています。

- 引数degreeにはプラスマイナス90度の範囲で角度を入れます。0がセンター位置。正の値を入れると時計回りに回ります。

- 引数speedは省略すると1になり、大きな値を入れるほどゆっくり動きます。0を入れると最速で動きますが、実行後に適切な長さのtime.sleep()を入れてあげないとプロセスが溜まって挙動がおかしくなってしまいます。

- サーボSG90は180度回転することができますが、ロボットなどの構造上、回転角度に制限をもたせたい場合もあるでしょう。そんなときはサーボごとにmaxとminを調整してください。

- サーボのセンタリング位置を微調整したい場合は、adjの値を変えてみてください。


サンプル


  • 上記bezelie.pyを含むサンプルコードはgithubで公開しております。

  • サンプルプログラムの使いかたについては、ベゼリー公式ページをご参照ください。


謝辞

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