Arduinoでライブラリ無しでサーボモーターを制御(PWM出力)する方法

 Arduinoで準備されているライブラリ関数(servo.h)を使用せずにサーボモーターを制御(PWM出力)する方法です。

 ライブラリは便利なのですが、いろいろ制限があるので今回はライブラリを使わずにPWM出力する方法(ソース、スケッチ)です。ライブラリを使う場合と使わない場合の両方を見ていきたいと思います。

スポンサーリンク

Arduinoのライブラリ(servo.h)を使用する場合

 まずはライブラリ(servo.h)を使う場合です。もともとArduinoには便利なライブラリ関数が準備されていて、ライブラリ使用すればかなり簡単にサーボ制御(パルス出力)を行うことができます。

▼一般的なサーボモーターの制御信号(PWM出力)▼

 HIGH時間の長さを変えることによってサーボの角度を制御します。こういったHIGH/LOW(ON/OFF)を繰り返し、HIGHの幅(長さ)を変化させるような信号をPWM出力とか言います。また、ここでいう500μs~2600μsのHIGH時間をパルス幅、パルスの間隔をPWM周期なんていう言い方もします。

 サーボにもよりますが、角度:0度~180度がだいたいパルス幅500~2600μsくらのHIGH時間に対応していてパルス幅を変えることで、サーボモータの角度を変えます。この信号をArduinoピンに接続したサーボモータに出力することで角度制御します。このパルス出力、いわゆるPWM出力はArduinoライブラリから簡単に生成することができます。

▼ライブラリ使用例▼

void setup() {
  myservo.attach(9);
  myservo.write(90);
}

▲この記述では、9ピンに接続されたサーボを90度の位置にします。ライブラリ使用すれば非常に簡単で、たったこれだけでサーボ制御のためのパルス信号を生成することができます。

 ですがライブラリを使用する場合少し制限があって、パルス周期(制御周期)は20ms固定であったり、9,10ピンのPWMが同時に使用できなかったりします。

 また複数ピンで同時にサーボ制御する場合も少し変わった出力します。

サーボパルス幅実測

 ライブラリを使用して3ピン同時にサーボ制御したときのパルス波形ですが、同時に出力せずに順番に行っているようです。

 特にこだわりというか問題が無ければライブラリを使用すればOKです。周波数変更したい場合や9,10ピンのPWMを同時に使用するとか、ピンの制限受けずに出力するなどは、ライブラリでは難しいです。なので今回は自前でPWM出力行ってみたいと思います。

スポンサーリンク

ライブラリを使わずにサーボ制御(PWM出力)

 なるべく汎用的に使いたいので、timerなどは使わずにloopの中で処理したいと思います。

 ソースを抜粋して説明してきます。全体は記事後半に掲載してます。

  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  currentTime = micros();
  loopTimer   = currentTime; 

▲まずはsetup内の記述です。今回は4個のサーボモーターを制御する想定で4ピン分を出力設定にしてます。最後の2行は時間を計測するための変数で後程説明します。

  while (1) {
    currentTime = micros();
    if (currentTime - loopTimer >= 4000)break; //250Hz(4000us)
  }
  loopTimer = currentTime;

▲ここからはloop内の処理です。まず最初に1ループの時間を決めます。これはそのままPWM出力の周波数(周期)になります。

 決めた時間になるまで無限ループさせてます。ここでは4000μsになったらループを抜け出すようにしてます。なので1ループを4000μs以内で処理できるようにする必要があります。この記述はloop内のどこに記述してもいいですが、ここではわかりやすく一番最初に記述してます。

  //PWM 3,5,6,7pin => HIGH
  digitalWrite(4, HIGH);
  digitalWrite(5, HIGH);
  digitalWrite(6, HIGH);
  digitalWrite(7, HIGH);
  PWMLoopTimer = micros();

 ▲ここからPWM出力を生成します。まず最初にPWM出力するピンを全て「HIGH」にします。その直後に現在の時間を測定し、変数に格納しておきます。

  //PWMパルス幅カウンターセット
  PWM1_timer = 1000 + PWMLoopTimer;//4pin
  PWM2_timer = 1250 + PWMLoopTimer;//5pin
  PWM3_timer = 1500 + PWMLoopTimer;//6pin
  PWM4_timer = 2000 + PWMLoopTimer;//7pin

▲次にHIGH時間(パルス幅)を設定します。「パルス幅(μs)」に、先ほどの「ピンをHIGHにしたときの時間」を足した値をそれぞれ変数に格納しておきます。ここでは4つのピンを順に1000μs、1250μs、1500μs、2000μsのパルス幅になるように設定してます。

  //PWM 3,5,6,9pin => LOW
  while (digitalRead(4) + digitalRead(5) + digitalRead(6) + digitalRead(7) > 0) {
    PWMLoopTimer = micros();
    if (PWM1_timer <= PWMLoopTimer)digitalWrite(4, LOW);
    if (PWM2_timer <= PWMLoopTimer)digitalWrite(5, LOW);
    if (PWM3_timer <= PWMLoopTimer)digitalWrite(6, LOW);
    if (PWM4_timer <= PWMLoopTimer)digitalWrite(7, LOW);
  }

▲最後に設定したパルス幅(時間)になったらピンを「Low」にします。全部のピンが「Low」になるまでループさせてます。またサーボ制御の場合、HIGHにしてからLOWにするまでの間に、500μs~1000μsほどの無駄時間が必ず発生するので、その間にできる他の処理を挟んでもいいと思います。

 これで最初に設定している制御(ループ)周期でPWM出力を行ってます。今回説明を簡単にするため、IDEの関数(digitalWrite/Read)使ってますが、処理に時間が掛かるのと、一度にpin操作できないため、実際のプログラムではポートを直接制御してます。

▼例えば▼

  PORTD |= B11110000;

 こんな感じで記述すれば4、5、6、7pinを同時にHIGHにできますし、digitalWrite()使用するより圧倒的に処理速度が速いです。

 ライブラリ使わずに、少し融通利かせた制御とかしたい場合とか、ピン制限受けたくない場合とか私はよくこの方法でPWM出力してます。

コメント