【RaspberryPi】ハードPWMではなくソフトPWMでサーボモータを制御 with C++

ラズパイで同時に複数のサーボを使いたかった！

ラズパイでpwmを出力しようとすると、GPIO18（物理12pin）を使うのがフツーだと思います。
これがググるといっぱい出てくるハードpwmで、ハード的にpwm波を作ってくれます。

しかし「いっぱいサーボを使うんだ！」ということで、他の普通のGPIOピンからソフト的にpwm波を作り、サーボを制御させてみました。

言語はC++です。
Python？だって…C++の方が画像処理が速いんだい…（今は関係ない）

結論から言っておくと、サーボはうまくいきませんでした。
±2度程度をキュイキュイ言ってます(; x ;)

コンテンツ

使ったもの

・raspberry pi zero　2でも3でも（多分4でも）変わらないです。
・sg90
　　【仕様】　　周期　：20 [ms]
　　　　　　　パルス幅：0.5～2.4[ms]
　　　　　　　回転角　：0～180 [度]

また以下の二つのライブラリを使いました。

・wiringPi.h
・softPwm.h

インストール方法はGoogle先生にお任せします。

配線

ラズパイ　　16　　　　　　39
　　　　　　｜　　　　　　｜
　sg90　　オレンジ　赤　　茶
　　　　　　　　　　｜　　｜
電池（5v）　　　　（＋）（－）

ラズパイはどちらも物理ピン番号です。

使った関数

int wiringPiSetup()

wiringPi.hよりGPIOピンの初期化です。
失敗すると-1を返します。

int softPwmCreate(int output_pin, int initial_phase, int palse_cycle)

softPwm.hよりソフトpwmを出力するピンのセットアップです。
成功で0を返します。
各パラメータは以下の通りです。

int output_pin
　出力するピンをwiringPi番号（BCM番号でも物理番号でもない）で指定。
　今回はwiringPi番号4（BCM番号23、物理番号16）を使用します。

int inital_phase
　…初期値を指定するらしい。
　らしい、というのもよく分かってないからです。
　とりあえず０にしておけ、と(^^;

int palse_cycle
　pwm波の周期を100[μs]単位で指定。
　サーボの仕様によるので、今回はsg90の仕様から200（20[ms]）を指定します。

void softPwmWrite(int output_pin, int palse_width)

softPwm.hよりソフトpwm波を出力します。
この関数で別にスレッドを立てて、そこで時間を刻んでパルス波を出力する感じみたいです。
各パラメータは以下の通りです。

int output_pin
　出力ピン。
　こっちもwiringPi番号でピンを指定するので、今回は4。

int palse_width
　パルス幅を100[μs]単位で指定。
　サーボはこのパルス幅によってどの位置を向くかが決まる。
　sg90なら0.5～2.4[ms]でそれぞれ0～180度を向く。

テストコード

0から180の入力を待ち、それに応じた角度を向かせるようにしました。
終了時は”end"と打てば終了です。

入力が数字でない時にstoi関数が例外を投げるので、例外処理を入れました。
その入力は無視して、再入力を求めるようになっています。

コードは以下のようになってます。

#include <wiringPi.h>
#include <softPwm.h>
#include <iostream>
#include <string>

using namespace std;

float computePalseWidth(int angle) {
   // Servo can position only 0-180
   angle = angle >   0 ? angle :   0;
   angle = angle < 180 ? angle : 180;
   // Palse-width must be 0.5-2.4[ms]
   return angle / 180.0 * 1.9 + 0.5;
}

int main() {
   const int PWM_OUT = 4; //wPi番号
   const int PALSE_RANGE = 20; //[ms]

   // Init
   wiringPiSetup();
   softPwmCreate(PWM_OUT, 0, 10*PALSE_RANGE);

   cout << "When you finish : end" << endl;
   while (true) {
      float palseWidth;

      try {
         string angle;
         cout << "(0 ~ 180) -> ";
         cin >> angle;
         if (angle == "end") break;

         palseWidth = computePalseWidth( stoi(angle) );

      } catch (...) { /* Skip */ continue; }

      softPwmWrite(PWM_OUT, 10*palseWidth);
      delay(500);
   }

   // Finalize
   // Do nothing
}

そういえば、ハードpwmの時には出力を停止してあげる必要がありましたね？
そこはソフトpwmですから。
プログラムが終了すると自動で出力はなくなり、終了時に忘れずにやらなきゃいけないことはないのです！

見たまんまですが、computePalseWidth()ではパルス幅を0.5～2.4[ms]で求めています。

結果！

駄目でした (omg!!

というのも、動くには動いたのですが、どうもパルス波が安定しない様子。
±2度くらいをキュイキュイ行ったり来たりです…。

配線が切れかかってるんだ！
と現実逃避気味にハードpwmでやってみると、お察しの通りピタっと止まってくれますね。

単純にソフトpwmの限界だろうと結論づけます ( ; ; )

ちなみに、地味にsoftPwmCreate()の第三引数とsoftPwmWrite()の第二引数が0.1[ms]単位なのに気づかず30分くらいはまりました。
intで表したい、かつ分解能がそんなに高くない、ということでのこの単位なのでしょうが…分かりにくいやい！

LEDとかの明るさを制御するためなら全然使えると思いますが、サーボでは使えませんね。
ラズパイで複数のサーボを別々に制御するために何か代案がないものか…要検討です、
といったところで、今回は終しまいです :)