ブラシ付モータ FA-130RA を Hブリッジ回路で PWM制御する (4) / 回転数を PID制御する

ブラシ付モータ FA-130RA-2270 を、Hブリッジ回路で駆動し、PWM で速度制御しています。今回は、回転数を一定にする制御を、PID制御でおこなってみます。

前回は、ギヤボックスの出力軸にエンコーダを取り付け、回転数をフィードバックできるようにしました。

ブラシ付モータ FA-130RA を Hブリッジ回路で PWM制御する (3) / エンコーダをつくる

ブラシ付モータ FA-130RA-2270 を、Hブリッジ回路で駆動し、PWM で速度制御しています。今回は、エンコーダを取り付け、回転数を検出できるようにします。前回までに、ブラシ付モータを回すための回路をつくり、Arduino のスケ...

Arduino のスケッチを描いていきましょう。

回転数を一定に制御するスケッチ

前に描いたスケッチを改修していきます。



// Brushed Dc-Motor PID Control v.0.97b 2023.1.24 meyon

//#define DEBUG_ON

class TM1630 {
private:
  const byte DISPLAY_MODE = 0x00;
  const byte WRITE_REG_AUTO_ADDR = 0x40;
  const byte WRITE_REG_FIXED_ADDR = 0x44;
  const byte READ_KEY_SCAN = 0x42;
  const byte SET_DISPLAY_ADDR = 0xC0;
  const byte DISPLAY_OFF = 0x80;
  const byte DISPLAY_ON = 0x88 | 0x84;
  const byte STB_ENABLE = LOW;
  const byte STB_DISABLE = HIGH;
  const int DOT_POSITION = 1;

  const byte digit[12] = {
    0b01111110,  // 0
    0b00001100,  // 1
    0b10110110,  // 2
    0b10011110,  // 3
    0b11001100,  // 4
    0b11011010,  // 5
    0b11111010,  // 6
    0b01001110,  // 7
    0b11111110,  // 8
    0b11011110,  // 9
    0b00000000,  // 10:blank
    0b00100000  // 11:dot
  };

  const byte dioPin = 10;
  const byte clkPin = 11;
  const byte stbPin = 12;

  const byte numberOfDigits = DISPLAY_MODE & 1 ? 5 : 4;
  const byte blank = 10;
  const byte dot = 11;

  byte gridData[];

回転数を表示するために 7セグメントLEDをつけました。そのドライバ TM1630 のクラス宣言です。これまでと変わりありません。小数点以下1位までを表示させます (16行)。



public:
  TM1630() {
    pinMode(dioPin, OUTPUT);
    pinMode(clkPin, OUTPUT);
    pinMode(stbPin, OUTPUT);

    gridData[numberOfDigits] = {0};

    digitalWrite(stbPin, STB_ENABLE);
    shiftOut(dioPin, clkPin, LSBFIRST, DISPLAY_MODE);
    digitalWrite(stbPin, STB_DISABLE);

    digitalWrite(stbPin, STB_ENABLE);
    shiftOut(dioPin, clkPin, LSBFIRST, WRITE_REG_AUTO_ADDR);
    digitalWrite(stbPin, STB_DISABLE);
  }

コンストラクタ。ここも変わりないです。



  void displayNumbers(int num) {
    int n = constrain(num, 0000, 9999);
    for (byte i = 0; i < numberOfDigits; i++) {
      int exponentialInDecimal = pow(10, i) + 0.5;
      bool zeroSuppression = (0 != i) && (DOT_POSITION < i) && (exponentialInDecimal > n);
      gridData[i] = zeroSuppression ? blank : n / exponentialInDecimal % 10;
    }

    digitalWrite(stbPin,STB_ENABLE);
    shiftOut(dioPin, clkPin, LSBFIRST, SET_DISPLAY_ADDR | 0x00);
    for (byte i = 0; i < numberOfDigits; i++) {
      shiftOut(dioPin, clkPin, LSBFIRST, digit[gridData[i]]);
      shiftOut(dioPin, clkPin, LSBFIRST, DOT_POSITION == i ? digit[dot] : 0);
    }
    digitalWrite(stbPin, STB_DISABLE);

    digitalWrite(stbPin, STB_ENABLE);
    shiftOut(dioPin, clkPin, LSBFIRST, DISPLAY_ON);
    digitalWrite(stbPin, STB_DISABLE);
  }
};

数値を表示するメンバ関数。
61行目、データ範囲を 0~9999 に制限しました。



class PulseDetector {
private:
  const byte rpmSensorPin = 2;

  const long detectionTimeout = 400000; // Detection timeout above this value (us)
  static const byte dataEntries = 10;   // Data entries of Moving average

public:
  PulseDetector () {
    pinMode(rpmSensorPin, INPUT);
  }

  long detectPulsePeriod() {
    long period = 0;

    static byte dataPoint = 0;
    static long periodData[dataEntries] = {0};
    static long sum = 0;
    long mean = 0;

    period = pulseIn(rpmSensorPin, HIGH, detectionTimeout);

    // Moving average
    sum -= periodData[dataPoint];
    periodData[dataPoint] = period;
    sum += periodData[dataPoint];
    mean = sum / dataEntries;

    dataPoint++;
    if(dataEntries <= dataPoint) dataPoint = 0;

    return mean;
  }
};

エンコーダのパルスを検出するクラス。
エンコーダの精度が悪いので、すこし悪あがきしています。本来なら、エンコーダ自体を改善すべきですね。

102行目。エンコーダ出力の HIGH時間だけを取りだしています。
pulseIn() では、連続した HIGHと LOWの信号を検出できないみたいなので、LOW時間も取り出そうとすると、2周期分の時間がかかってしまいます。なので、HIGH時間だけ検出し、制御しています。
回転数は、オンデューティ比で割って計算することにしました。でも、オンデューティ比も一定ではないので、まぁ、表示はテキトーですなぁ (；´Д｀)

検出値のバラツキが大きいので、フィルタリングのために移動平均をとることにしました。
104~111行目が移動平均の計算。アルゴリズムは簡単です。合計値から古い値を引いて、新しい値を足す。毎回合計を計算する必要はありません。dataEntries を大きくすればバラツキは改善されますが、応答性が悪くなります。



class BrushedMotor {
private:
  const byte potentiometerPin = A0;
  const byte forwardPin = 6;
  const byte reversalPin = 7;
  const byte pwmOutputPin = 5;

  const int forwardArea = 532;  // Forward above this value (0~1023)
  const int reversalArea = 491; // Reversal below this value (0~1023)
  const int hysteresis = 10;    // Hysteresis of Area
  const int minimumSetVariable = 100; // Voltage at startup (0~255)
  const int maximumSetVariable = 255; // Voltage at maximum speed (0~255)
  const long minimumPulsePeriod = 25000; // Period at minimum speed (us)
  const long maximumPulsePeriod = 65000; // Period at maximum speed (us)

  const float Kp = 0.8;   // Proportional
  const float Ki = 0.14;  // Integral
  const float Kd = 3.2;   // Differential

ブラシ付モータのクラス。
メンバ変数は定数の定義です。ピン番号と、制御パラメータなど、いろいろ。

forwardArea、reversalArea は、ボリュームの出力がそれぞれこの値以上のとき正転、以下のとき逆転します。hysteresis は、ボリューム入力に与えるヒステリシス。起動停止が、ちょうどしきい値のときのばたつきを抑制します。各レベルの範囲が交錯しないように設定してください、ませ。

minimumSetVariable、maximumSetVariable は、モータ電圧出力の範囲です。100 でモータ電圧 0.9V、255 で最大電圧になります。minimumPulsePeriod、maximumPulsePeriod は、それぞれ最高速時、最低速時のエンコーダ出力パルスの HIGH時間。この範囲で PID制御します。
Kp、Ki、Kd は、PID制御の係数です。この値を決めるのが、なかなか難しいですよねぇ。いろいろ思いはあるけれど、今回はこんな数値に決めました。



public:
  BrushedMotor() {
    pinMode(forwardPin, OUTPUT);
    pinMode(reversalPin, OUTPUT);
  }

コンストラクタは、ピンモードの設定。



  void obtainSetPoint(bool *p_setFwd, bool *p_setRev, int *p_setDutyRi) {
    bool forward = LOW;
    bool reversal = LOW;
    int inputValue = 0;
    int dutyRatio = 0;
    static int hys = 0;
    int thldValue = 0;

    inputValue = analogRead(potentiometerPin);
    thldValue = inputValue + hys;

    if(forwardArea <= thldValue) {
      forward = HIGH;
      reversal = LOW;
      dutyRatio = map(inputValue, forwardArea, 1023, 0, 255);
      hys = hysteresis;
    }
    else if (reversalArea >= thldValue) {
      forward = LOW;
      reversal = HIGH;
      dutyRatio = map(inputValue, 0, reversalArea, 255, 0);
      hys = -hysteresis;
    }
    else {
      forward = HIGH;
      reversal = HIGH;
      dutyRatio = 0;
      hys = 0;
    }

  *p_setFwd = forward;
  *p_setRev = reversal;
  *p_setDutyRi = constrain(dutyRatio, 0, 255);
}

ボリュームの位置から、回転方向と速度を取得するメンバ関数。

前のスケッチと基本的に変わりはないですが、ヒステリシスをつけました。正転の領域ではプラスの値を、逆転の領域ではマイナスの値を入力値に加算しています。じつは、ちょっとビミョーなところがあるんですけど、まぁ気にしない、しない。



  int pidControl(int setDutyRi, long pulsePrd) {
    int manipVar = 0;
    int manipOutput = 0;

    long SV = 0;
    long PV = 0;
    static long en = 0;
    static long en1 = 0;
    long en2 = 0;
    static long MVn = 0;
    long MVn1 = 0;
    long dMVn = 0;

    SV = map(setDutyRi, 0, 255, maximumPulsePeriod, minimumPulsePeriod);

    if(0 == setDutyRi | 0 == pulsePrd) {
      PV = maximumPulsePeriod;
    } else {
      PV = pulsePrd;
    }

    // Velocity PID control
    en2 = en1;
    en1 = en;
    en = SV - PV;
    MVn1 = MVn;

    dMVn = Kp*(en-en1) + Ki*en + Kd*((en-en1)-(en1-en2));
    MVn = MVn1 + dMVn;

    manipVar = map(MVn, maximumPulsePeriod, minimumPulsePeriod, 0, 255);
    manipOutput = constrain(manipVar, 0, 255);

#ifdef DEBUG_ON
  Serial.print("SV:");
  Serial.print(SV);
  Serial.print(",");
  Serial.print("PV:");
  Serial.print(PV);
  Serial.print(",");
  Serial.print("MVn:");
  Serial.print(MVn);
  Serial.print("\n");
#endif

    return manipOutput;
  }

課題の PID制御のメンバ関数。
これまでと同じように、速度型制御アルゴリズムを使いました。

結果でいうと、60~120rpm の回転数では、まぁうまくいってます。応答が遅いので、目標値を急に変化させるとすこし振動したりもしますが、いい感じだと思います。
問題は、停止から 55rpm あたりの低速域。50rpmほどで回していると、微妙な振動がでることがあります。特に、停止から起動させるとき振動して、安定するのに時間がかかります。制御係数を小さくすることで安定させることもできますが、そうすると高速域での応答が悪くなります。
エンコーダの精度をあげるとともに、起動時の制御は違った方法を考えないといけないのかもしれません。

177行目。引数は、ボリュームの位置から設定された出力目標値と、エンコーダからの回転周期です。
190行目。出力目標値を回転周期に換算して、PID制御の目標値としています。
192~196行目、モータ停止時に、操作量 MVn がどんどん増えていくのを抑えています。
198~205行が PID制御アルゴリズム。
207~208行は、操作量をモータ電圧出力値に変換して、戻します。
210~220行は、シリアルプロッタへの出力です。3行目のコメントを外すと出力します。



  void operateMotor(int setFwd, int setRev, int manipVar) {
    int manipDuty = 0;

    manipDuty = map(manipVar, 0, 255, minimumSetVariable, maximumSetVariable);

    digitalWrite(forwardPin, setFwd);
    digitalWrite(reversalPin, setRev);
    analogWrite(pwmOutputPin, manipDuty);
  }
};

モータ制御部ドライバへ制御信号を出力するメンバ関数です。



TM1630 ledDisplay;
PulseDetector pulseDetector;
BrushedMotor brushedMotor;

void setup() {

#ifdef DEBUG_ON
  Serial.begin(9600);
#endif

  }

void loop() {
  long pulsePeriod = 0;
  int manipulatedVariable = 0;
  bool setForward = LOW;
  bool setReversal = LOW;
  int setDutyRatio = 0;
  int numberToDisplay = 0;

  static const unsigned long interval = 1000;
  static unsigned long previousMillis = 0;
  unsigned long currentMillis = 0;

  currentMillis = millis();

  brushedMotor.obtainSetPoint(&setForward, &setReversal, &setDutyRatio);
  pulsePeriod = pulseDetector.detectPulsePeriod();
  manipulatedVariable = brushedMotor.pidControl(setDutyRatio, pulsePeriod);
  brushedMotor.operateMotor(setForward, setReversal, manipulatedVariable);

  if(interval <= currentMillis - previousMillis) {
    if(1 == setForward && 1 == setReversal | 0 == pulsePeriod) {
      numberToDisplay = 0;
    } else {
      numberToDisplay = 31200000 / pulsePeriod; // (*1)
    }
    ledDisplay.displayNumbers(numberToDisplay);

    previousMillis = currentMillis;
  }
}

オブジェクトの生成と、loop()関数。

モータ制御の周期は loop()の速さに任せています。パルスの検出に時間がかかるので、エンコーダ出力をすべて捉えてはいないようです。
7セグメントLED の表示は 1秒おき (interval) に行ないます。リアルタイムで表示させる必要もないですし、モータ制御の邪魔にもなりますから。268~269行目は、停止時に範囲外の数値が表示されるのを抑止しています。特に、pulsePeriod が 0 になると除算がエラーになってしまいます。



/* ----------------------------------------------------------------

  (*1)Factor for finding RPM from pulsePeriod.
      RPM is to one decimal place.

  Encoder duty = 52%
  Encoder pulses = 10 [/round]
  T = (pulsePeriod / duty) * pulses [us]
  PRM = 60 * 1000000 / T [rpm]

  F = 0.52 * 10 * 1000000 * 60 / 10 = 31200000
---------------------------------------------------------------- */

最後に、回転数表示への換算式 (271行) についてのメモ。
回転数パルスのオンディーティ比を 52%、1回転あたり 10パルスとして、パルスの HIGH時間から回転数 (rpm) を計算する係数 F を求めています。7セグメントLED には小数点以下 1位まで表示しますので、求めた回転数を 10倍しています。