ServoBlasterを使えばRaspberry Piでサーボを動かせるけど、コマンドで操作するのでC++からは使いにくかった。
Raspberry PiのGPIOの18番ピンからPWM出力を出せるみたいなのでそれを使って動かしてみた。
使ったもの
- Raspberry Pi Model B
- SG90（サーボ）
- WiringPi
サーボの制御の仕組み
PWM信号とかサーボの仕組みがよく分かってなかったので、まずはこのページを読んで勉強した。
H8で学ぶマイコン開発入門（9）：モータ制御に欠かせない技術“PWM”って何？ (1/3) - MONOist（モノイスト）
一定の周期とパルス幅を持ったPWM信号を送るとサーボを制御できるみたいだ。
SG90の仕様
今回使うサーボはSG90の仕様は次の通り。
これを見るとこのサーボは180°回転できて、そのためには周期：20ms、パルス幅：0.5～2.4msのPWM信号を出力すればよいのが分かる。
※ 2のサイトにはパルス幅1～2msと書いてあるけど、両方試したら1の方が正しいみたいだった。
WiringPiでサーボを制御する方法
今回GPIOの制御にはWiringPiを使うことにした。
WiringPiにはPWM出力用の関数があるので、これらを使うとサーボ制御に適したPWM信号を出力できる。
Raspberry Pi Specifics | Wiring Pi
PWM出力用の関数は次の3つ。
- pwmSetMode(int mode);
- pwmSetRange(unsigned int range);
- pwmSetClock(int divisor);
WiringPiのリファレンスを読んでも使い方がよく分からなかったが、このサイトの回答が参考になった。
gpio - Control Hardware PWM frequency - Raspberry Pi Stack Exchange
要点をまとめると、
- WiringPiはデフォルトでBalancedモードなのでMark:Spaceモードに設定する必要がある。
- PWM周波数を次の式で計算できて、これがサーボの周期と一致するようにclockとrangeを設定する。
pwm frequency=19.2[MHz]/clock/range
- 19.2MHzというのはRaspberry PiのPWMが持つベースクロックの周波数。
- ベースクロックをclockで割った値（$=19.2[MHz]/clock$）はPWMのカウンタの周期を表し、この周期でPWMのカウンタが1だけインクリメントされる。
- rangeはPWMの分解能に影響する値。
ということらしい。ベースクロックについてはよく分からないけど、信号のOn / Offを切り替えるタイミングが1秒間に$19.2*10^6$回あるってことだと思う。
あとはclockとrangeをSG90の仕様に合わせて決定すればいいんだけど、どの値がいいか分からなかったのでこのサイトの値を使ってみた。
Controlling Two Servos with Hardware PWM on the Raspberry Pi Model A+ – electronut.in
$clock=400,range=1024$を代入すると、
- PWMのカウンタ周期：$19.2[MHz]/clock=19.2*10^6/400=48[KHz]≒0.0208[ms]$
- PWM周波数：$19.2[MHz]/clock/range=19.2*10^6/400/1024=46.875[Hz]≒21.3[ms]$
になった。
SG90のPWMの周期が20msだからPWM周波数が少し低いみたいだけど、動かしてみたら問題なかった。アバウトでいいらしい。
次にサーボを端から端まで回転させるための値を計算する。
SG90の仕様だとパルス幅：0.5～2.4msなので、
- $48[KHz]*0.5[ms]=48000[Hz]*0.0005[s]=24$
- $48[KHz]*2.4[ms]=48000[Hz]*0.0024[s]=115.2≒115$
この値をWiringPiで設定すればサーボを左右に限界まで回転させられる。
Raspberry Piでサーボを制御する
Raspberry PiとSG90は、橙⇒GPIO18 / 赤⇒5V / 茶⇒GND と配線する。
WiringPiを使ってサーボを動かすプログラム次の通り。
整数値を入力して、その値をGPIO18に設定する。
#include <iostream>
#include <wiringPi/wiringPi.h>
int main()
{
if (wiringPiSetupGpio() == -1) {
std::cout << "cannot setup gpio." << std::endl;
return 1;
}
pinMode(18, PWM_OUTPUT);
pwmSetMode(PWM_MODE_MS);
pwmSetClock(400);
pwmSetRange(1024);
while (true) {
int num;
std::cin >> num;
if (num == -1) {
break;
}
pwmWrite(18, num);
}
return 0;
}
結果はこちら。
最初に24を入力、次に115を入力してる。
WiringPi Servo Sample | Youtube
動画だと180°ちょっと回ってるけど、サーボは大抵仕様より少し大きい範囲まで回転できるらしいので上の計算は合ってるみたいだ。
無事サーボを動かせた。
余談
最初にWiringPiのexapmleにPWM出力のサンプルプログラム（pwm.c）があってこれを使えば簡単にできると思ったんだけど、このサンプルではサーボの制御はできなかった。
試しに動かしてみたらサーボに付けたアームが振り切って動かせなくなって、買ったばかりのサーボ壊した・・・！？と思って焦った。大丈夫だったけど。
サンプルのPWMのモードはBalancedモードになっていて、このモードはLEDの明るさを調整したりするときに適したモードらしい。