タヒチ　モーレア島の夕日

エデン・プロジェクト

エデン

それはまだ見ぬ自律航行型の水上ヴィラ

Raspberry Piでサーボを動かす

自律航行のボートを作る前に、まずはラジコンボートを作ろうと思うわけですが、モーター・サーボ・GPS・コントローラー・カメラあたりが最初に思いつくところかなと。モーターの制御からやろうかなとも思ったのですが、ブラシレスモーターを使いたくて、ちょっと高いんですよね。なので最初はサーボの制御から習得していくことにしました。

動作原理

サーボモーターはPWMというパルス信号によって制御します。通常はPWMの周期は50Hz、つまり20ms間隔です。この周期の中でどのくらいの間電気を流すかによって角度を決めています。
0.5msから2.5msの間で0°から180°まで動かします。
1.5msなら90°でニュートラルです。
1.5msを中央にして、±1msです。

結線

このサーボはオレンジ・赤・茶色の3つの線があります。
以下のように結線しました。
オレンジ（SIG）⇒オレンジ⇒GPIO18
赤（VCC）⇒赤⇒3V3
茶色（GND）⇒黒⇒GND

コード

コードは以下のような形にしました。
/run/Servo18.target ファイルが書き換わるとサーボも追随して動きます。

WEBアプリケーションで/run/Servo18.statusファイルを書き換える想定です。

#!/usr/bin/env python3

import RPi.GPIO as GPIO
import time
import sys

SERVO_MIN_PULSE = 500
SERVO_MAX_PULSE = 2500

ServoPin = 18
STEP = 1  # 1回で動かす角度
INTERVAL = 0.002  # 1回動かすのに待つ秒数 PWMの周期(20ms)以下だとバグる
L_INTERVAL = 0.1  # メインループのインターバル
S_FILE = '/run/Servo18.status'  # 今の角度を保存しているファイル
T_FILE = '/run/Servo18.target'  # 今の角度を保存しているファイル
args = sys.argv # 未使用

def map(value, inMin, inMax, outMin, outMax):
    return (outMax - outMin) * (value - inMin) / (inMax - inMin) + outMin

def setup():
    global p
    GPIO.setmode(GPIO.BCM)       # Numbers GPIOs by BCM
    GPIO.setwarnings(False)          # 正しく終了しなかったときのWarning抑止
    GPIO.setup(ServoPin, GPIO.OUT)   # Set ServoPin's mode is output
    GPIO.setwarnings(True)           # 初期化時以外はWarningを出力
    GPIO.output(ServoPin, GPIO.LOW)  # Set ServoPin to low
    p = GPIO.PWM(ServoPin, 50)       # set Frequecy to 50Hz
    p.start(0)                       # Duty Cycle = 0

def move(angle):
    angle = max(0, min(180, angle))
    start = 0
    target = 0

    with open(S_FILE, 'r', encoding='utf-8') as f:
        start = int(f.read()) // STEP * STEP  # 半端な角度をSTEPに揃える
    if angle < start :  # 時計回り
        target = angle - 1
        move_STEP = STEP * -1
    elif angle > start:  # 反時計回り
        target = angle + 1
        move_STEP = STEP
    else:  # 等しいとき
        target = angle
        i = angle
        move_STEP = STEP

    for i in range(start, target, move_STEP):
        # 角度をパルス幅に変換
        pulse_width = map(i, 0, 180, SERVO_MIN_PULSE, SERVO_MAX_PULSE)
        # パルス幅をデューティー比に変換
        pwm = map(pulse_width, 0, 20000, 0, 100)
        # デューティー比を変更
        p.ChangeDutyCycle(pwm)
        with open(S_FILE, 'w', encoding='utf-8') as f:
            f.write(str(i))
        time.sleep(INTERVAL)
    p.ChangeDutyCycle(0)  # 動かし終わったらパルスを止める

def loop():
    while True:
        target = 0
        status = 0
        with open(T_FILE, 'r', encoding='utf-8') as f:
            target = int(f.read()) // STEP * STEP  # 半端な角度をSTEPに揃える
        with open(S_FILE, 'r', encoding='utf-8') as f:
            status = int(f.read()) // STEP * STEP  # 半端な角度をSTEPに揃える
        if target == status:
            time.sleep(L_INTERVAL)
            continue
        else:
            move(target)
            time.sleep(L_INTERVAL)

if __name__ == '__main__':     # Program start from here
    setup()
    try:
        loop()
    except KeyboardInterrupt:  # When 'Ctrl+C'
        pass
    p.ChangeDutyCycle(0)
    p.stop()
    GPIO.cleanup()

Posted

in

,

by

アダム

Tags: