ラズパイで半固定抵抗とADコンバーターでサーボモーターを制御するプログラム | そう備忘録

ラズパイで半固定抵抗とADコンバーターでサーボモーターを制御するプログラム

サーボモーターの制御

Raspberry Pi 3 B+からサーボモーターを制御するプログラムを作成した時の備忘録。

サーボモーターは指定した角度だけ動かすことができるモーターでロボットのアームやラジコンの車のステアリング等に使われている。

半固定抵抗のつまみを回すと回した角度に応じてサーボモーターを同じ方向へ回転させる回路とプログラミングを行ったので記事で残しておく。

サーボモーターをラズパイで制御する

サーボモーター

使用したサーボモーターは秋月電子通商で購入したマイクロサーボ9g SG-90

サーボモーター SG90

スペックは以下の通り。

PWMサイクル

20mS(=50Hz)

50HzのPWM信号でモータを制御する

制御パルス

0.5ms~2.4ms

以下データシートより

-90度

0.5ms

0度

1.45ms

90度

2.4ms

1.45msとは1.45ms÷20ms×100=7.5%のデューティ比のPWM信号でサーボモーターの位置を0度にするという意味

同様に0.5ms÷20ms×100=2.5%(-90度)

2.4ms÷20ms×100=12%(90度)

となる

制御角

±約90度(180度)

配線

GND

電源[+]

制御信号(PWM)

トルク

1.8kgf・cm

動作速度

0.1秒/60度

動作電圧

4.8V(~5V)

その他の主なパーツ

半固定抵抗

つまみを回すことで抵抗値を変えることが出来る抵抗。

つまみを回す方向と角度と同期してサーボモーターを同じ角度だけ回転させる為に使用する。

内部の2つの抵抗値を変更することで出力する電圧を変えている(抵抗の分圧計算)。

反時計回りに目一杯回すと電圧はゼロ、時計回りに目一杯回すと入力した電圧(今回は3.3V)が出力される。

尚、半固定抵抗や分圧計算についての詳細はこちらの記事を参照して欲しい。

ADコンバーター

Raspberry Piでは直接アナログ値を扱えないので半固定抵抗からの電圧(0~3.3V)をアナログ値からデジタル値に変換して0~4095の数値で取得する為に使用している。

MCP3208を利用している。

その他Raspberry Pi本体などについては以前の記事の「必要なモノ」を参考にして欲しい。

回路図

中央の10KΩの半固定抵抗のつまみを回すとそれに同期してサーボモーターが回転する。

半固定抵抗からの電圧をADコンバーター(MCP3208)のチャンネル0を介して0~4095のデジタル値としてSPI経由でRaspberry Piに取り込まれる。

12番ピン(GPIO18)はRaspberry Piに2つある精度の高いPWM信号を出力できる端子の一つ。

サーボモーターの黄色(信号線)に接続している。

Raspberry Piから精度の高いPWM信号を出力できるピンは以下の通り。

12番ピン

PWM0(GPIO18)※32番ピンとは同時に使用できない

32番ピン

PWM0(GPIO12)※12番ピンとは同時に使用できない

33番ピン

PWM1(GPIO13)※35番ピンとは同時に使用できない

35番ピン

PWM1(GPIO19)※33番ピンとは同時に使用できない

サーボモーター制御の回路図

電源は単三電池×4本を直列接続で1.5V×4本=6.0V。

サーボモーターの動作電圧が4.8Vなのだがテスターで電圧を測定した所、電池が放電していて5V以下だったのでそのまま繋いでいる。

参考書籍

回路図はいつもの事だが金丸隆志さんの「Raspberry Piで学ぶ電子工作」を参考にさせて貰った。

この書籍と秋月電子通商の「Raspberry Piで学ぶ電子工作 パーツセット」を一緒に購入して最初から最後まで書籍の内容通りに組み立てたのだがRaspberry Pi初心者にとっては非常に参考になった。

尚、上記の書籍はRaspberry Pi 3をベースに書かれている。

Raspberry Pi 4対応の書籍も出版されていたのでリンクを載せておく。

事前準備

ADコンバーターからの信号をSPI通信で受信するのでRaspberry Piの設定のインターフェースタブでspiを有効化する。

また前述の書籍ではPythonとRaspberry Piの学習のために基本的な命令でデバイスを操作しているのだがPythonからはspidevライブラリーを使ったほうが操作が簡単なのでこの記事のプログラムではspidevをインストールする。

SPIの有効化とspidevのインストール手順についてはこちらの記事を参照して欲しい。

pigpio

精密なPWM信号の出力にはプログラム中ではpigpioを使っている。

前述の紹介した書籍ではWiringを使っているのだがWiringは2020年現在開発が止まっているのでpigpioでプログラミングした。

pigpioにはhardware_PWMというメソッドがありハードウェアPWM(前述のピンを使用)で精密なPWM信号を出力出来る。

pigpioのPythonのドキュメントはこちらを参照して欲しい。

インストール

以下のコマンドでpigpioをインストールする。

sudo apt-get install pigpio

デーモンを起動する

pigpioはプログラムの実行前に管理者権限でデーモンを動かしておく必要がある。

sudo pigpiod

プログラム

サーボモーターをRaspberry Piから制御するPythonのプログラムは以下の通り。

ソースコード

# -*- coding: utf-8 -*-
"""
Created on Sun Aug 16 18:51:43 2020

・サーボモータの角度制御
・PWMの制御にはpigpioを使っている

@author: Souichirou Kikuchi
"""

import spidev
from time import sleep
import pigpio # PWM制御用モジュール

CHN = 0 # ADコンバーター接続チャンネル
PWM0 = 18 # Servo Motor PWM0

pwm0 = pigpio.pi()
pwm0.set_mode(PWM0, pigpio.OUTPUT)
spi = spidev.SpiDev()
spi.open(0, 0) # 0:SPI0、0:CE0
spi.max_speed_hz = 1000000 # 1MHz SPIのバージョンアップによりこの指定をしないと動かない

def get_data(channel):
    dout = spi.xfer2([((0b1000+channel)>>2)+0b100,((0b1000+channel)&0b0011)<<6,0]) # Din(RasPi→MCP3208)を指定
    bit12 = ((dout[1]&0b1111) << 8) + dout[2] # Dout(MCP3208→RasPi)から12ビットを取り出す
    return float(bit12) # 0~4095

def servo_duty_hwpwm(val): # ADコンバーターの値0~4095よりDuty比を計算する
    MAX_DUTY = 120000 # 12% 100%:1×1000×1000=1,000,000(100%)
    DIF_DUTY = 95000 # 12%-2.5%=9.5%=95000
    MAX_VAL = 4095 # valの最大値
    duty = int(MAX_DUTY - (DIF_DUTY / MAX_VAL * val))
    return duty

try:
    print('--- start program ---')
    while True:
        val = get_data(CHN)
        print('val= ',val)
        duty = servo_duty_hwpwm(val)
        print('duty= ',duty)
        pwm0.hardware_PWM(PWM0, 50, duty) # 50Hz
        sleep(0.2)
except KeyboardInterrupt:
    pass
finally:
    pwm0.stop()
    spi.close()
    print('--- stop program ---')

補足説明

モジュールのインポート

11~13行目でspidevやpigpioなど必要なモジュールをインポートしている。

定数の定義

15~16行目は定数の定義。

CHNはADコンバーターの接続チャンネルで一番左の0チャンネルに接続している。

PWM0はGPIO18(12番ピン)の18を指定している。

ADコンバーターからのデータ取得

24~27行目のget_data関数はADコンバーターからの数値(0~4095)を取得する関数。

Raspberry PiからDinを送るとDoutを12ビットで受け取る。

ビットのシフトを使ってDin、Doutを組み立てているので詳細は以前のこちらの記事を参照して欲しい

デューティ比の計算

29~34行目のservo_duty_hwpwm関数は半固定抵抗の数値(0~4095)からデューティ比を計算する関数。

サーボモーターの角度とデューティ比、半固定抵抗の数値(0~4095)の関係は以下の通り。

サーボモーターの角度

デューティ比・半固定抵抗

-90度(反時計回り)

デューティ比:2.5%(前述の制御パルスを参照)

半固定抵抗値:0

0度

デューティ比:7.5%

半固定抵抗値:2048

90度(時計回り)

デューティ比:12%

半固定抵抗値:4095

つまみを反時計回りに目一杯回して半固定抵抗からの電圧値0の時にデューティ比2.5%のPWM信号、2048(つまみが真ん中)の時に7.5%のデューティ比、4095の時に12%のデューティ比を出力するように計算をしている。

尚、hardware_PWMでは100%は1.0×1,000×1,000=1,000,000になるので2.5%だと25,000になる。

最大の12%と最小の2.5%の差異(9.5%)を4095で割ると半固定抵抗の数値1に対する比率が計算できるので最大値の12%から引き算をすることでPWMのデューティ比を計算している。

例外処理

36行目以降がメインの処理。

ry~except ~finallyで例外処理を行っている。

exceptではCtrl+Cでプログラムを終了した時に発生する例外を拾って後続の処理を実行するようにしている。

finallyではプログラム終了時に使用したリソースを開放している。

メインループ

38~44行目がメインのループ処理で0.2秒間隔でループしている。

39行目のget_data(CHN)でADコンバーターからの半固定抵抗値を0~4095で受け取る。

41行目のservo_duty_hwpwm(val)で上記の値からハードウェアPWMのデューティ比を計算している。

pwm0.hardware_PWM(PWM0, 50, duty)でservo_duty_hwpwmで計算したデューティ比を精密なPWM信号としてPWM0(GPIO18)に出力している。

実行結果

動画

サーボモーターの実際に動く様子は下記の動画を確認してみて欲しい。

半固定抵抗のつまみの回転に合わせてサーボモーターが動く様子が分かる。

画面ショット

Raspberry Pi上でpwm-servo.pyをThonnyで実行した時の画面ショット。

プログラム中のprint文でval、dutyが表示されている。

サーボモーター制御プログラム実行結果

以上で今回の記事は終了とする。

最後に

この記事が何処かで誰かの役に立つことを願っている。

尚、当記事中の商品へのリンクはAmazonアソシエイトへのリンクが含まれています。Amazonのアソシエイトとして、当メディアは適格販売により収入を得ていますのでご了承ください。

souichirou

やった事を忘れない為の備忘録 同じような事をやりたい人の参考になればと思ってブログにしてます。 主にレゴ、AWS(Amazon Web Services)、WordPress、Deep Learning、RaspberryPiに関するブログを書いています。 仕事では工場に協働ロボットの導入や中小企業へのAI/IoT導入のアドバイザーをやっています。 2019年7月にJDLA(一般社団法人 日本デイープラーニング協会)Deep Learning for GENERALに合格しました。 質問は記事一番下にあるコメントかメニュー上部の問い合わせからお願いします。

おすすめ

質問やコメントや励ましの言葉などを残す

名前、メール、サイト欄は任意です。
またメールアドレスは公開されません。