【ラズパイ電子工作】超音波センサに応じてサーボモータを制御する方法

ラズベリーパイは基板上に「GPIO」と呼ばれる、ラズベリーパイで作成したプログラムから信号の入力・信号の出力が行えるピンが存在します。

信号の入力とは「スイッチのON/OFF」「温度計で室温の計測」といった、ラズベリーパイの外の情報を入力(インプット)してプログラム上で使用することです。

対して、信号の出力とは「LEDを点灯させる」「モータを回す」といった、ラズベリーパイで制御した結果を外に出力(アウトプット)することです。

この記事では、ラズベリーパイのプログラム(Python)から超音波センサで距離を測定して、PWM出力でサーボモータの角度を制御する方法を解説します。

注意

この記事中に記載されている内容はソースコード含めて電気設計人が自己流で行ったものです。一般的な方法とは相違がある可能性がありますので予めご了承ください。

超音波センサで距離を測定する方法は以下のページで解説しておりますので、宜しければご覧ください。

【ラズパイ電子工作】超音波センサで距離を読み取る方法(HC-SR04)

サーボモータの角度を制御する方法は以下のページでも解説しております。

【ラズパイ電子工作】サーボモータを動かす方法(デューティ比指定)

【ラズパイ電子工作】サーボモータを動かす方法(角度指定)

【ラズパイ電子工作】スイッチを用いてサーボモータを動かす方法

【ラズパイ電子工作】ボリュームに応じてサーボモータを制御する方法(SPI通信)

1. 完成イメージ（超音波センサに応じてサーボモータの制御）

この記事で完成するものは以下のようになります。

超音波センサでiPhoneまでの距離を測定して、距離に応じてサーボモータの角度を制御します。今回は超音波センサで5～14cmの距離を測定します。

2. 使用する部品

今回使用する部品は以下の通りです。

ラズベリーパイ本体（モデル3B+）

『Raspberry Pi 3 Model B+』を使用します。※2020年7月時点で最新はRaspberry Pi 4になります。

ブレッドボード

LEDや抵抗といった各種部品や(後述する)ジャンパ線などを穴に差し込み、部品間を電気的に接続する板(ボード)です。

電子工作をする上で必須の部品です。

ジャンパ線

ブレッドボートに差し込み、電子部品の間を電気的に接続します。

↑の写真では両側が「ピン」になっており、ブレッドボートの穴に差し込んで使用します。

超音波センサ（HC-SR04）

HC-SR04は、超音波が反射する時間を利用して非接触で距離を測定するモジュールです。

「トリガー」ピンの状態をHighにすると超音波パルスが送信され、物体に反射して戻ってきた信号を受信します。超音波パルスを『送信した時間』と『戻ってきた信号を受信した時間』の差分を求めることで距離を測定します。

ラズパイのGPIOにて「トリガー」ピンを出力、「エコー」ピンを入力に割付けて使用します。

HC-SR04の主要なスペックは以下のようになります。

測定範囲	：2～400cm
電源電圧	：DC5.0V
動作電流	：15mA
動作周波数	：40kHz
トリガー信号	：10μs

サーボモータ（SG90）

サーボモータ(Servo motor)とは、位置制御や速度制御ができるモータです。

製造業においてサーボモータは、位置決め・速度制御やトルク制御を行う機構によく用いられる大変高額なものですが、今回はラジコンやホビーロボットに用いられる安価で小型なサーボモータ：SG90を使用します。

サーボモータ：SG90の主要なスペックは以下のようになります。

PWMサイクル	：20ms（50Hz）
制御パルス	：0.5~2.4ms
制御角度	：±90°（180°）
動作電圧	：4.8V（～5V）
動作スピード	：0.1s / 60°

動作角度が±90°のため、モータをグルグル回すことはできません。

抵抗器

LEDなどの電子部品は適した電圧と電流値が決められており、既定の範囲を超えると壊れてしまう恐れがあります。

抵抗器は電流の流れを妨げ、電子部品に流れる電流を抑える役割を担います。

今回は『超音波センサに1kΩ』の抵抗器を2ヶ使用します。

オススメの電子工作セット

電子工作をするため、これまで解説した部品の他にもスイッチやセンサなど、色々な部品が必要になってきます。個々で購入するには手間がかかるため、最初はセット品を購入することをおススメします。

電気設計人

私は以下のセット品を購入しました！

リンク

また、これからラズベリーパイを購入する場合、ラズベリーパイ本体を含めたセット品を購入することをおススメします。

リンク

ラズベリーパイ本体を収めるケースや、OSをインストールするためmicroSDカードなど必要なものを個々に購入する手間を省くことができます。

3. 回路図・配線の様子

ラズベリーパイのプログラム(Python)から超音波センサで距離を測定して、PWM出力でサーボモータの角度を制御する回路を解説します。

回路図

回路図は以下のようになります。

サーボモータの「赤色リード線を5Vピン」「茶色リード線をGNDピン」「オレンジ色リード線をGPIO 18番ポート」に接続します。

超音波センサHC-SR04には以下の4ヶのピンがあります。

VCC：DC5.0V電源（一番左側）
Trig：トリガー信号
Echo：エコー信号
GND：グランド（一番右側）

ラズベリーパイとHC-SR04を以下のように接続します。

HC-SR04	ラズベリーパイ	ジャンパ線
VCC	：5V	：赤色
Trig	：GPIO 27番ポート	：青色
Echo	：GPIO 23番ポート※抵抗経由	：黄色
GND	：GND	：黒色

Trigに接続するGPIO 27番ポートはラズベリーパイからトリガーを出力するため出力モードとして使用します。

対して、Echoに接続するGPIO 23番ポートはHC-SR04から”受信した”信号をラズベリーパイが入力するため入力モードとして使用します。

HC-SR04の動作電圧は5Vに対して、ラズベリーパイのGPIOは3.3Vになります。Echoをラズベリーパイがそのまま受けると、GPIOが破損する恐れがあります。そこで、抵抗器を用いて分圧します。

メモ

抵抗器に極性はありませんので、プラスとマイナスは気にしなくてOKです。

配線の様子

配線の様子です。こんな感じになりました。

↑では、フラットケーブルでGPIOの全ピンをブレッドボードに接続しています。回路図と実際の配線は異なる部分がありますがご了承ください。

※電気的には「回路図」と同じ意味です。

4. プログラム(Python)

超音波センサで距離を測定して、PWM出力でサーボモータの角度を制御するプログラム(Python)は以下のようになります。

ソースコード

#必要なモジュールをインポート
import RPi.GPIO as GPIO             #GPIO用のモジュールをインポート
import time                         #時間制御用のモジュールをインポート
import sys                          #sysモジュールをインポート

#ポート番号の定義
Trig = 27                           #変数"Trig"に27を代入
Echo = 23                           #変数"Echo"に23を代入
Servo_pin = 18                      #変数"Servo_pin"に18を格納

#GPIOの設定
GPIO.setmode(GPIO.BCM)              #GPIOのモードを"GPIO.BCM"に設定
GPIO.setup(Trig, GPIO.OUT)          #GPIO27を出力モードに設定
GPIO.setup(Echo, GPIO.IN)           #GPIO23を入力モードに設定
GPIO.setup(Servo_pin, GPIO.OUT)     #GPIO18を出力モードに設定

#PWMの設定
#サーボモータSG90の周波数は50[Hz]
Servo = GPIO.PWM(Servo_pin, 50)     #GPIO.PWM(ポート番号, 周波数[Hz])
Servo.start(0)                      #Servo.start(デューティ比[0-100%])

#角度からデューティ比を求める関数
def servo_angle(angle):
    duty = 2.5 + (12.0 - 2.5) * (angle + 90) / 180   #角度からデューティ比を求める
    Servo.ChangeDutyCycle(duty)     #デューティ比を変更

#HC-SR04で距離を測定する関数
def read_distance():
    GPIO.output(Trig, GPIO.HIGH)            #GPIO27の出力をHigh(3.3V)にする
    time.sleep(0.00001)                     #10μ秒間待つ
    GPIO.output(Trig, GPIO.LOW)             #GPIO27の出力をLow(0V)にする

    while GPIO.input(Echo) == GPIO.LOW:     #GPIO23がLowの時間
        sig_off = time.time()
    while GPIO.input(Echo) == GPIO.HIGH:    #GPIO23がHighの時間
        sig_on = time.time()

    duration = sig_on - sig_off             #GPIO23がHighしている時間を演算
    distance = duration * 34000 / 2         #距離を求める(cm)
    return distance

#連続して値を超音波センサの状態を読み取る
while True:
    try:
        cm = read_distance()                   #HC-SR04で距離を測定する
        if cm > 5 and cm < 14:                 #距離が5〜14cmの場合
            position = cm * 20 - 190           #5～14を角度(-90～90)に変換
            print(position)                    #位置決めする角度を表示
            servo_angle(position)              #PWM信号出力(角度は変数"position")
        time.sleep(0.01)                       #0.01秒間待つ

    except KeyboardInterrupt:       #Ctrl+Cキーが押された
        Servo.stop()                #サーボモータをストップ
        GPIO.cleanup()              #GPIOをクリーンアップ
        sys.exit()                  #プログラム終了

超音波センサHC-SR04の「トリガー」ピンにGPIOからHighに出力して、「エコー」ピンに信号が戻ってきた時間を計測して距離を求めます。

測定した距離が5～14cmの範囲内であればサーボモータに出力するPWM信号のデューティ比を変えることで角度を制御します。

プログラムの解説

#必要なモジュールをインポート
import RPi.GPIO as GPIO             #GPIO用のモジュールをインポート
import time                         #時間制御用のモジュールをインポート
import sys                          #sysモジュールをインポート

2～4行目で今回必要な「モジュール」を宣言します。

#ポート番号の定義
Trig = 27                           #変数"Trig"に27を代入
Echo = 23                           #変数"Echo"に23を代入
Servo_pin = 18                      #変数"Servo_pin"に18を格納

変数”Trig”と”Echo”と”Servo_pin”に数値を代入します。この数値はGPIOのポート番号として扱い、後からポート番号を変更する場合はこの数値を変更します。

今回は『GPIO 27番ポートをトリガー』『GPIO 23番ポートをエコー』『GPIO 18番ポートをサーボモータ』として使用します。

#GPIOの設定
GPIO.setmode(GPIO.BCM)              #GPIOのモードを"GPIO.BCM"に設定
GPIO.setup(Trig, GPIO.OUT)          #GPIO27を出力モードに設定
GPIO.setup(Echo, GPIO.IN)           #GPIO23を入力モードに設定
GPIO.setup(Servo_pin, GPIO.OUT)     #GPIO18を出力モードに設定

GPIOの設定を行います。

GPIO.setmode(GPIO.BCM)は、GPIOをポート番号で扱う方法に設定します。

GPIO.setup(Trig, GPIO.OUT)は、GPIO 27番ポートを出力モードに設定します。

GPIO.setup(Echo, GPIO.IN)は、GPIO 23番ポートを入力モードに設定します。

GPIO.setup(Servo_pin, GPIO.OUT)は、GPIO 18番ポートを出力モードに設定します。

#PWMの設定
#サーボモータSG90の周波数は50[Hz]
Servo = GPIO.PWM(Servo_pin, 50)     #GPIO.PWM(ポート番号, 周波数[Hz])
Servo.start(0)                      #Servo.start(デューティ比[0-100%])

PWM信号を使用する為の設定を行います。

Servo = GPIO.PWM(Servo_pin, 50)は、GPIO 18番ポートを50HzのPWM信号に割り当てます。

Servo.start(0)は、GPIO 18番ポートのPWM信号のデューティ比を0%に出力します。

#角度からデューティ比を求める関数
def servo_angle(angle):
    duty = 2.5 + (12.0 - 2.5) * (angle + 90) / 180   #角度からデューティ比を求める
    Servo.ChangeDutyCycle(duty)     #デューティ比を変更

サーボモータを任意の角度に動作させるためには、角度からデューティ比を計算で求める必要があります。

逐一計算からデューティ比を求めても問題ありませんが、今回は指定された角度を基にPWMサイクルのデューティ比を求める関数を作成します。

今回の関数servo_angle(angle)は、引数(angle)に角度を指定することで、関数内でデューティ比dutyを求めます。

求めたデューティ比は、Servo.ChangeDutyCycle(duty)を用いてGPIO 18番ポートのデューティ比を変更します。

例えば、引数(angle)に”-90”が指定された場合、dutyを求める式により、duty = 2.5になります。SG90はデューティ比を2.5%にすることで約-90°に動作します。

#HC-SR04で距離を測定する関数
def read_distance():
    GPIO.output(Trig, GPIO.HIGH)            #GPIO6の出力をHigh(3.3V)にする
    time.sleep(0.00001)                     #10μ秒間待つ
    GPIO.output(Trig, GPIO.LOW)             #GPIO6の出力をLow(0V)にする

    while GPIO.input(Echo) == GPIO.LOW:     #GPIO25がLowの時間
        sig_off = time.time()
    while GPIO.input(Echo) == GPIO.HIGH:    #GPIO25がHighの時間
        sig_on = time.time()

    duration = sig_on - sig_off             #GPIO25がHighしている時間を演算
    distance = duration * 34000 / 2         #距離を求める(cm)
    return distance

HC-SR04にて距離を測定する関数を作成します。

まずHC-SR04の「トリガー」ピンに対して、10μ秒間信号をHighにします。(29～31行目)

次に「エコー」ピンの状態を監視して、LowからHighになるまでの時間を測定します。(33～36行目)

最後に距離を求める演算を行い、変数”distance”を戻り値に返します。

distance = duration * 34000 / 2は、測定した時間を音速で掛けて半分にしています。音速とは約340m/秒であるため、cmに直すと34000となります。

#連続して値を超音波センサの状態を読み取る
while True:
    try:
        cm = read_distance()                   #HC-SR04で距離を測定する
        if cm > 5 and cm < 14:                 #距離が5〜14cmの場合
            position = cm * 20 - 190           #5～14を角度(-90～90)に変換
            print(position)                    #位置決めする角度を表示
            servo_angle(position)              #PWM信号出力(角度は変数"position")
        time.sleep(0.01)                       #0.01秒間待つ

    except KeyboardInterrupt:       #Ctrl+Cキーが押された
        Servo.stop()                #サーボモータをストップ
        GPIO.cleanup()              #GPIOをクリーンアップ
        sys.exit()                  #プログラム終了

「while文」は条件を指定して、その条件が真の時に繰り返し処理を行うものです。

Python

while 条件式:
    繰り返し処理を行うコード

↑の「条件式」にTrueを指定することにより、条件式は常に真となりwhile文は無限に繰り返します。

メモ

無限に繰り返すことを、無限ループと表現したりします。

ただし、このままではプログラム実行中に無限ループから抜け出す方法がありません。そこでwhile文の中にある「try文」と「except文」を使用します。

このtry-except文を用いることにより、while文の処理は以下のようになります。

Python

while True:
    try:
        繰り返し処理を行うコード
    except KeyboardInterrupt:               #Ctrl+Cキーが押された
        Servo.stop()                        #サーボモータをストップ
        GPIO.cleanup()                      #GPIOをクリーンアップ
        sys.exit()                          #プログラム終了

except KeyboardInterrupt:は、キーボードのCtrl + cキーが押された時にwhile文の繰り返し処理から抜けて上記のプログラム終了の処理を行います。

Ctrl + cキーが押されていないとき、try:文の中の処理を繰り返し行います。(以下で解説)

while文の中の「繰り返し処理を行うコード」は以下の通りです。つまり、Ctrl + cキーが押されるまで以下の処理を繰り返します。

    cm = read_distance()                   #HC-SR04で距離を測定する
    if cm > 5 and cm < 14:                 #距離が5〜14cmの場合
        position = cm * 20 - 190           #5～14を角度(-90～90)に変換
        print(position)                    #位置決めする角度を表示
        servo_angle(position)              #PWM信号出力(角度は変数"position")
    time.sleep(0.01)                       #0.01秒間待つ

cm = read_distance()は、HC-SR04で測定した距離を変数”cm”に代入します。

今回はHC-SR04で測定した距離が5～14cmの場合に赤色バーLEDを制御するため、if cm > 5 and cm < 14:にて範囲外の値が入力された場合は除外する処理を行います。

測定した距離が5～14cmの場合、position = cm * 20 - 190にてサーボモータを動作させる角度(-90～90)に変換して、servo_angle(position)にて関数に渡します。

time.sleep(0.01)は何もせずに0.01秒間待ちます。