Raspberry Piでサーボコントロール、Part3 Raspberry Pi 2に対応

Raspberry Pi 2 model Bが登場して、GPIOのアドレスが変更された関係で、金魚サイトのサーボコントローラー(上下左右ズーム機能)が動いていませんでした。

対処法も出てきたので重い腰を上げてPi2でもサーボがコントロールできるように修正します。

WebIOPiを再コンパイルする

まずWebIOPiは現在Pi2に対応していませんが、ソースコードを少し入れ替えることでPi2に対応するようになります。

やりかたは下記です。

wget http://sourceforge.net/projects/webiopi/files/WebIOPi-0.7.1.tar.gz/download
tar xvzf download
cd WebIOPi-0.7.1/python/native

mv cpuinfo.c cpuinfo.c.original
mv gpio.c gpio.c.original

wget http://denshikousaku.net/wp-content/uploads/2015/02/cpuinfo.c
wget http://denshikousaku.net/wp-content/uploads/2015/02/gpio.c

cd ../..
sudo ./setup.sh

Do you want to access WebIOPi over Internet? [y/n]
というような項目が出てくるけどnを選択する。これはWeaved用の設定なので無視する。

WebIOPiの設定はネット経由(WebIOPi)でコントロールすると同じ感じでOK。

これでWebIOPiがRaspberry Pi 2に対応しました。ただB+で追加されたGPIOがうまく認識されないとかされるとか。とりあえず1ピンから26ピンまでを使いましょうー。

2015年3月か4月くらいにはWebIOPiが正式にPi2に対応すると思うので、それまで仮対応で対処しておくのが無難だと思います。

RPIOを放棄してWebIOPiでサーボをコントロール

上記でWebIOPiはPi2に対応したけど、RPIOがPi2に対応していませんでした。WebIOPiと同じ感じでcpuinfo.cとc_gpio.cを修正して再コンパイルしてみましたが、起動はするもののGPIOが反応しませんでした。

RPIOの最終アップデートが2年前。。近いうちにPi2に対応してくれるとは思えないのでサーボをコントロールするのにRPIOを使うのはやめて、コードを書き変えます。

pulseAngleとpulseRatioを使って無理やりサーボをコントロールすることにしました。pulseRatioが必要なのか微妙ですが、サーボがアル中みたいにビクビクな感じになってしまうような、ということでpulseRatioを入れています。無駄なコードが若干ある気がしますが、気にしない！

その結果のコード

sudo vim servo-script.py

import webiopi
import time

GPIO = webiopi.GPIO
SERVO_X = 22
SERVO_Y = 24
SERVO_Z = 25
DEFAULT_X = -10
DEFAULT_Y = -49
DEFAULT_Z = -17
CURRENT_X = DEFAULT_X
CURRENT_Y = DEFAULT_Y
CURRENT_Z = 0

time_stamp = time.time()

def setup():
    GPIO.setFunction(SERVO_X, GPIO.PWM)
    GPIO.setFunction(SERVO_Y, GPIO.PWM)
    GPIO.setFunction(SERVO_Z, GPIO.PWM)

GPIO.pulseAngle(SERVO_X, DEFAULT_X)
GPIO.pulseAngle(SERVO_Y, DEFAULT_Y)
GPIO.pulseAngle(SERVO_Z, DEFAULT_Z)
webiopi.sleep(0.5)
GPIO.pulseRatio(SERVO_X, 0)
GPIO.pulseRatio(SERVO_Y, 0)
GPIO.pulseRatio(SERVO_Z, 0)

def loop():
   global time_stamp
   if (time_stamp <= time.time() - 20):
       defaultPosition()

   webiopi.sleep(0.5)

@webiopi.macro
def defaultPosition():
    global time_stamp
    if time_stamp <= time.time() - 0.2 and (CURRENT_X != DEFAULT_X or CURRENT_Y != DEFAULT_Y or CURRENT_Z != 0):
        time_stamp = time.time()
        global CURRENT_X
        global CURRENT_Y
        global CURRENT_Z
        CURRENT_X = DEFAULT_X
        CURRENT_Y = DEFAULT_Y
        GPIO.pulseRatio(SERVO_X, 1)
        GPIO.pulseAngle(SERVO_X, DEFAULT_X)
        webiopi.sleep(0.5)
        GPIO.pulseRatio(SERVO_X, 0)
        GPIO.pulseRatio(SERVO_Y, 1)
        GPIO.pulseAngle(SERVO_Y, DEFAULT_Y)
        webiopi.sleep(0.5)
        GPIO.pulseRatio(SERVO_Y, 0)
        GPIO.pulseRatio(SERVO_Z, 1)
        GPIO.pulseAngle(SERVO_Z, DEFAULT_Z-10.7)
        webiopi.sleep(CURRENT_Z*1.05)
        GPIO.pulseRatio(SERVO_Z, 0)
        CURRENT_Z = 0
        time_stamp = time.time()

@webiopi.macro
def forward():
    global CURRENT_Z, time_stamp
    if CURRENT_Z < 10 and time_stamp <= time.time() - 0.2:
        GPIO.pulseRatio(SERVO_Z, 1)
        time_stamp = time.time()
        CURRENT_Z = CURRENT_Z + 1
        GPIO.pulseAngle(SERVO_Z, DEFAULT_Z+15)
        webiopi.sleep(1)
        GPIO.pulseAngle(SERVO_Z, DEFAULT_Z)
        time_stamp = time.time()
        GPIO.pulseRatio(SERVO_Z, 0)

@webiopi.macro
def backward():
    global CURRENT_Z, time_stamp
    if CURRENT_Z > 0 and time_stamp <= time.time() - 0.2:
        GPIO.pulseRatio(SERVO_Z, 1)
        time_stamp = time.time()
        CURRENT_Z = CURRENT_Z - 1
        GPIO.pulseAngle(SERVO_Z, DEFAULT_Z-10.7)
        webiopi.sleep(1*1.05)
        GPIO.pulseAngle(SERVO_Z, DEFAULT_Z)
        time_stamp = time.time()
        GPIO.pulseRatio(SERVO_Z, 0)

@webiopi.macro
def down():
    global CURRENT_Y, time_stamp
    if CURRENT_Y < DEFAULT_Y + 5*10 and time_stamp <= time.time() - 0.2:
        GPIO.pulseRatio(SERVO_Y, 1)
        time_stamp = time.time()
        CURRENT_Y = CURRENT_Y + 5
        GPIO.pulseAngle(SERVO_Y, CURRENT_Y)
        webiopi.sleep(0.5)
        time_stamp = time.time()
        GPIO.pulseRatio(SERVO_Y, 0)

@webiopi.macro
def up():
    global CURRENT_Y, time_stamp
    if CURRENT_Y > DEFAULT_Y - 5*10 and time_stamp <= time.time() - 0.2:
        GPIO.pulseRatio(SERVO_Y, 1)
        time_stamp = time.time()
        CURRENT_Y = CURRENT_Y - 5
        GPIO.pulseAngle(SERVO_Y, CURRENT_Y)
        webiopi.sleep(0.5)
        time_stamp = time.time()
        GPIO.pulseRatio(SERVO_Y, 0)

@webiopi.macro
def left():
    global CURRENT_X, time_stamp
    if CURRENT_X < DEFAULT_X + 5*10 and time_stamp <= time.time() - 0.2:
        GPIO.pulseRatio(SERVO_X, 1)
        time_stamp = time.time()
        CURRENT_X = CURRENT_X + 5
        GPIO.pulseAngle(SERVO_X, CURRENT_X)
        webiopi.sleep(0.5)
        time_stamp = time.time()
        GPIO.pulseRatio(SERVO_X, 0)

@webiopi.macro
def right():
    global CURRENT_X, time_stamp
    if CURRENT_X > DEFAULT_X - 5*10 and time_stamp <= time.time() - 0.2:
        GPIO.pulseRatio(SERVO_X, 1)
        time_stamp = time.time()
        CURRENT_X = CURRENT_X - 5
        GPIO.pulseAngle(SERVO_X, CURRENT_X)
        webiopi.sleep(0.5)
        time_stamp = time.time()
        GPIO.pulseRatio(SERVO_X, 0)

def destroy():
    GPIO.setup(SERVO_X, GPIO.IN)
    GPIO.setup(SERVO_Y, GPIO.IN)
    GPIO.setup(SERVO_Z, GPIO.IN)

これで金魚サイトのWebカメラの上下左右ズーム機能が動くようになりました。

ツイート