ブログ

ブログです

ただいまシステムの中身③　GPIOを使ったサーボモータ制御

Raspberry Pi 趣味

前回までで命令系統が完成したから、いよいよサーボモータ制御についてご紹介。今回使うサーボモータはPWM (Pulse Width Modulation, パルス幅変調)制御のやつ。
f:id:hira-hide:20170429171428p:plain
そもそもPWMってなんだよって人のために軽く説明すると、上の図みたいに一定周期(T_cycle)である幅(T_duty)だけ電圧を高くするようなやつのこと。このT_dutyを短くしたり長くしたりすると、それに応じてサーボモータが動く仕組み。

今回の流れは、まずGPIOコントロール用ソフトのWiring Piってやつをインストールして、その後にサーボ制御用プログラムを書く感じ。

今回使う「GWSサーボ　PIC/STD/F」ってやつはいっくら探してもデータシートが見当たらなかったんだけど、ダメ元でSG-90と同じ値使ったらうまく動いたから助かった。

T_cycle T_duty V_high
20ms 0.5 - 2.4ms 4.8 - 5.0V

T_duty=0.5msが-90度、2.4msが90度に対応する感じ。

参考ページ

今回の参考ページはこちら。
hombre-nuevo.com
hombre-nuevo.com
http://blog.enjel.chips.jp/?eid=303blog.enjel.chips.jp

Wiring Piのセットアップ

まずはI2Cライブラリのインストールをして、その後GitでWiringPiのソースをダウンロードしてインストール、最後にWiringPi2をインストールする流れ。
なんかよく分からんけど、Raspberry Pi3でWiringPiをpythonから呼び出すときはWiringPi2のほうが良いらしい。

I2Cライブラリインストール

Raspberry Pi3のGPIOピンのうち3番、5番、27番、28番はI2Cに使えるピンで、それのセットアップのために必要なライブラリのインストール。

$ sudo apt-get install libi2c-dev

WiringPiのソースダウンロードとインストール

Gitが入ってなかったらまずはGitのインストールから。

$ sudo apt-get install git-core

そしたらGit cloneでソースコードを持ってくる。

$ git clone git://git.drogon.net/wiringPi

wiringPiっていう名前のディレクトリでコピーされるから、そこに移動してビルド。

$ cd wiringPi
$ ./build

これでWiringPi自体はインストール完了なので、WiringPi2のセットアップ。

$ sudo apt-get update
$ sudo apt-get install python3-dev python3-pip
$ sudo pip3 install wiringpi2
$ git clone https://github.com/Gadgetoid/WiringPi2-Python.git
$ cd WiringPi2-Python
$ sudo python3 setup.py install

これでOK。

サーボモータを動かす

Raspberry Piでサーボモータを動かすためには最初に書いたようにPWMを出力しなきゃいけないんだけど、Raspberry Pi3でPWM出力にハードウェアで対応してるGPIOピンは下図のPWMxと書かれてるピンのみ。
f:id:hira-hide:20170507123830p:plain
それ以外のピンでPWMをやろうとするとソフトウェア的にやらなきゃいけなくてめんどうだから、PWM対応ピンを使うのが無難（一応WiringPiの機能でSoftware PWMもあるらしい）。

さて、WiringPiでサーボモータを動かす時には pwmWrite っていう関数を使う。

void pwmWrite(int pin, int value);
Writes the value to the PWM register for the given pin. The Raspberry Pi has one on-board PWM pin, pin 1 (BMC_GPIO 18, Phys 12) and the range is 0-1024. Other PWM devices may have other PWM ranges.

引数にピン番号（int pin）と動かしたい角度（int value）を入力するんだけど、動かしたい角度は直接角度を入力するんじゃなくて「T_cycleのうちの何割をT_dutyに充てるか」を用いて指定する。
例えば「90度動かしたいなぁ」ってときは、上で書いた表から計算すると 2.4ms/20ms=12% と計算できる。
さらに、上の関数説明文の中に and the range is 0-1024 って書いてあるように、デフォルトだと 0=0%, 1024=100% を意味するようになってるから、最終的に与える引数は int(12*1024/100) となる。

サーボ制御プログラム

さーてここまで来たらあとはプログラムを書いて動かすだけだ！

#!/usr/bin/python

import wiringpi2 as wp
import sys

servo_pin = 12    # 12番ピンを指定
param = sys.argv
set_degree = int(param[1])
print(set_degree)

wp.wiringPiSetupGpio()    # 上図 pin(BOARD) の番号でピン指定するモード
wp.pinMode(servo_pin, 2)  # 出力ピンとして指定
wp.pwmSetMode(0)          # 0Vに指定
wp.pwmSetRange(1024)      # レンジを0～1024に指定
wp.pwmSetClock(375)       # 後述

move_deg = int((4.75*set_degree/90 + 7.25)*(1024/100)) # 後述
wp.pwmWrite(servo_pin, move_deg)

実行時の引数に角度を指定してこのプログラムを実行すると、指定した角度だけちゃんとサーボが動いてくれる。

さて、上で後述とした二点について。wp.pwmSetClock() はWiring PiのPWM制御のための基準周波数を決める関数で、以下の条件式が成立するらしい。

PWM周波数＝Raspberry PiのPWMが持つベースクロックの周波数／（clock×レンジ）

いま、サーボのPWM周波数が50Hz、Raspberry PiのPWMが持つベースクロックの周波数が19.2MHz、レンジが1024だから、clock=18750/50=375 になる。
move_deg = int( (4.75*set_degree/90 + 7.25)*(1024/100) ) ではサーボ動作の引数を計算してるんだけど、ここでは「0度を中心として何度動かすか？」を計算している。
0度のときが7.25%、90度のときが12%、-90度のときが2.5%だから、その間を線形に計算するような式を突っ込んでるだけ。

これで音声認識、サーボ動作、2つのコアとなる機能ができたから、あとはこいつらをまとめ上げればただいまシステムは完成だ！