Parallax Feedback 360°High-Speed Servoの角度を制御するプログラム

制御工学
2020.05.25

みなさん，こんにちは
おかしょです．

ロボットを作ろうとすると，サーボの角度を細かく制御したくなることがあります．

この記事ではサーボの角度を制御する方法を解説します．

この記事を読むと以下のようなことがわかる・できるようになります．

  • Parallax Feedback 360°High-Speed Servoの使い方
  • サーボの角度の制御の仕方
  • P制御とは

　

この記事を読む前に

この記事は以下の記事の続きとなっています．

サーボの角度を制御するには，まずサーボの角度を取得する必要があります．
以下の記事では角度を取得する方法を解説しているので，以下の記事を先に読んでおくことをおすすめします．

Parallax Feedback 360°High-Speed Servoから角度を取得するプログラム
みなさん，こんにちは．おかしょです． この記事で使用するサーボはサーボの角度をセンサーで取得し，フィードバックすることができます．この機能を利用して，サーボの角度を指定して制御することが可能です． この記事ではサーボから角度を取得する方法に...
okasho-engineer.com
2020.05.24

　

P制御とは

さて，今回は角度の制御をP制御で行おうと思います．
そこで，まずはP制御の解説を簡単にしておこうと思います．詳しくはこちらの記事で解説しているので，参照してください．

P制御というのは比例制御とも呼ばれ，目標値と現在の値との差を取り，その差に対して一定値を掛けた値を入力として与える制御器を言います．

この制御の特徴は，ゲインと呼ばれる一定値を大きくすればするほど即応性が早くなります．しかし，大きくしすぎるとオーバーシュートと呼ばれる行き過ぎが生じてしまいます．また，入力が飽和してしまうこともあります．反対にゲインが小さすぎると，定常偏差が大きくなってしまうため目標値に到達しなくなってしまうという特徴があります．

ゲインの調整を誤ると，機械によっては壊してしまう事もあるので注意しましょう．

　

角度の制御をするプログラム

実際に角度の制御ができるプログラムを公開しますが，この記事では制御の部分のみ公開します．

　

変数の定義

まずは変数の定義を行います．

#include <Servo.h>
Servo servo;

float angle = 90.0;
int Kp = 1;

最初にServo.hというヘッダファイルをインクルードします．Arduinoの場合は，最初からServo.hはダウンロードされているので，プログラムでインクルードするだけでOKです．
angleというのは角度の目標値を表しています．KpはP制御のゲインのことです．
今回はサーボの角度を90°にする制御を行います．

　

setup()関数

ここでは，サーボの制御の信号を送るピンの設定を行います．

void setup()
{
  servo.attach(2);
}

私は2番ピンを制御信号線と接続するようにしました．

　

loop()関数

この関数内では，制御入力の決定と制御信号の送信を行います．
まず，制御入力の決定は以下のようにして行います．

float error = angle - theta;
int u = -1*Kp*(int)(error);

まず，目標値と現在の値の偏差を計算します．その結果がfloat型のerrorに入ります．
その誤差に対して，ゲインKpを掛けて制御入力uとします．

データシートを確認すると，制御入力はある一定の値を超えると一定の角速度しか出ないようになっています．
そこで，以下のようにして入力uがある一定値を超えたら切り捨てるようにします．

if(u>200)
{
  u = 200;
}
else if(u<-200)
{
  u = -200;
}

また，実際に与える制御信号はこの頭打ちになる値の中間値を基準とします．今回の場合，頭打ちになる値は1280と1720なので，基準値は1500となります．
この基準値ではサーボの角速度は0となるのですが，角速度を0とする入力には幅があるため制御入力はその分，オフセットを以下のように与える必要があります．

if(u>0)
{
  u = u+40;
}
else if(u<0)
{
  u = u-40;
}

このようにして制御入力を決定できたら，制御信号の送信を行います．

servo.writeMicroseconds(1500+u);

以上のようにすることで，サーボの角度の制御をすることができます．

　

まとめ

最後に，角度の算出から制御までのすべてを行えるプログラムを以下に示します．動作確認も済んでいるのでぜひ参考にしてみてください．

#include <Servo.h>
Servo servo;

#define InputPin  3

float theta = 0;
float thetaP = 0;
bool turn = true;
unsigned long tHigh, tLow, tLowS, tLowF;
float duty;

int count = 0;

float angle = 90.0;
int Kp = 1;

void setup()
{
  Serial.begin(9600);
  pinMode(InputPin, INPUT);
  servo.attach(2);
  tLowS = micros();
  tLowF = micros();
  tHigh = 0;
  tLow = 0;
}

void loop()
{
  if(digitalRead(InputPin)==LOW && turn)
  {
    tLowS = micros();
    tHigh = tLowS-tLowF;
    turn = false;
  }
  else if(digitalRead(InputPin) && turn==false)
  {
    tLowF = micros();
    tLow = tLowF-tLowS;
    unsigned long t = tHigh+tLow;
    if(t>1000 && t<1200)
    {
      thetaP = theta-360*count;
      duty = 100*tHigh/(tHigh+tLow);
      theta = (duty-2.9)/(97.1-2.9)*360;
      if(thetaP>270 && theta <90)
      {
        count++;
      }
      else if(thetaP<90 && theta>270)
      {
        count--;
      }
      theta = 360*count+theta;
      Serial.println(theta);
      float error = angle - theta;
      int u = -1*Kp*(int)(error);
      if(u>200)
      {
        u = 200;
      }
      else if(u<-200)
      {
        u = -200;
      }

      if(u>0)
      {
        u = u+40;
      }
      else if(u<0)
      {
        u = u-40;
      }

      servo.writeMicroseconds(1500+u);
    }
    theta = theta;
    turn =  true;
  }
}

この記事ではサーボの角度を制御する方法について解説しました．

制御器はP制御としましたが，問題なく制御できます．

　

続けて読む

そもそも，私がこのサーボを購入したのはセンサーのキャリブレーションを自動で行うキャリブレーションロボットを作成するためです．

このキャリブレーションロボットは地磁気センサーのキャリブレーションをするために，センサーをすべての方向に向ける必要があるのでこのようにサーボの角度を制御する必要があります．

このキャリブレーションロボットの詳しい説明は以下の記事で行っているので，興味のある方は読んでみてください．

センサーキャリブレーション用ロボット（構想編）
みなさん，こんにちはおかしょです． 地磁気センサーに限らず，センサーのキャリブレーションを行うにはセンサーをさまざまな方向に向ける必要があります．以前まではセンサーを持ってぐるぐる回したり，椅子の上で自分が回りながら手に持っているセンサーを...
okasho-engineer.com
2020.05.17

Twitterでは記事の更新情報や活動の進捗などをつぶやいているので気が向いたらフォローしてください．

それでは最後まで読んでいただきありがとうございました．

コメント

タイトルとURLをコピーしました