見出し画像

ステッピングモータのストール検出

mkbtm

ステッピングモータは指定したパルス数だけ回転するモータです。通常は指定した角度だけ回転して正確な位置決めに使えます。しかしストール（脱調）すると位置がずれてしまい位置合わせに問題が生じます。
またストール時には不快な音が発生します。それを防ぐためにもストール検出を行い対策をしたいところです。

ステッピングモータドライバDRV8434A

ステッピングモータのストール検出にはロータリーエンコーダを使い回転角度をチェックする方法があります。しかしその場合には部品点数が増えます。
追加のセンサ無しでストール検出ができるドライバもあります。L6470とDRV8434A。この2つが手軽に入手できる価格帯のドライバになります。
L6470はI2Cで接続して多数のモータを制御するときなどに便利な機能があり、回転角度や速度を指定して制御ができます。ただ使い方がちょっと独特です。
DRV8434AはA4988などのドライバと同形状でパルス入力の一般的なタイプです。
pololuやスイッチサイエンスで購入可能です。

DRV8434Aについてはテキサスインストルメンツの技術資料が参考になります。

日本語の資料：

英語版：
https://www.ti.com/lit/ds/symlink/drv8434a.pdf?ts=1679511911853

こちらの資料にもありますが、簡単にいうとモータに入力してる電圧と逆起電力の差を監視してそれが0に近づくとストール状態と判定するという事になります。
いくつかモードがありますが、私が試したのは「トルクカウントモード」の中の「MCU を使ったストール検出モード」になります。スタンドアロンモードでは学習のプロセスが必要らしいのでMCU（Arduino）を使ったストール検出モードを試しました。

ストール検出の準備

TRQ_CNT/ STL_THにモータの入力電圧と逆起電力のズレが電圧として出力されます。この値が0に近いとストールしていると判断できます。まずは正常動作からストールさせて電圧の変化を観察します。マイクロステップが細かいとこの変化が小さいらしいので1/4のマイクロステップにしています。

テスターで正常回転とストールした時のTRQ_CNT/ STL_THピンに出力される電圧を確認しておきます。

DRV8434Aの「TRQ_CNT/ STL_TH」ポートにカウント値が電圧として出力されます。私が試したモータでは正常に回転している場合には0.8V程度。軸に触れて負荷をかけると0.3V程度。完全に軸を止めた時には0.1V程度になりました。閾値を0.2〜0.3Vぐらいに設定してストールと判定します。モータの種類、回転速度によっても変わる値なので自分の環境に合わせましょう。

方法１：Arduinoのアナログ入力を使う

TRQ_CNT/ STL_THピンの出力電圧をArduinoのアナログ入力に接続して電圧を読み込みます。値が一定以下の場合にはストールと判定します。追加部品無し、コードで閾値を決めることができます。ただ、この方法の欠点はArduinoのアナログポートに加えられた電圧を読み取るためのAnalogRead()の処理が遅いです。
ステッピングモータのパルスを送る作業と並行して行うとパルスが間に合わなくなる場合があります。使う場面によっては難しそうです。そして利点の逆なのですが閾値の設定にはコードを書き換える必要があり現場ですぐに調整したい場合にやりにくいという欠点があります。

Analog入力を使ったストール検出

Arduino Pro miniを使ってますが、UNOでもなんでも同じでしょう。あと動作確認用にストール時に光るLEDを入れてありますが、図では省略しています。
スケッチは次のとおりです。

const int pulsePin = 3;
const int dirPin = 2;
const int led = 5;
const int sensorPin = A0;

int sensorValue;



void setup() {
  pinMode(pulsePin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(led, OUTPUT);

  delay(10);
  digitalWrite(pulsePin, LOW);
  digitalWrite(dirPin, LOW);
  digitalWrite(led, LOW);
}



void loop() {
    digitalWrite(pulsePin, HIGH);
    delayMicroseconds(2);
    digitalWrite(pulsePin, LOW);
    delayMicroseconds(200);

    sensorValue = analogRead(sensorPin);

    if (sensorValue < 80) {
        digitalWrite(led, HIGH);

        delay(1000);
        digitalWrite(led, LOW);
    }

}

loop内でステッピングモータドライバにパルスを送っています。そしてA0の値をanalogRead(sensorPin）で読み込んでいます。A0から読み込んだ値が80以下つまり
　5.0V/1024 = 0.0048828125
　0.0048828125 x 80 = 0.39V
TRQ_CNT/ STL_THピンの電圧が0.39V以下の時にストールと判定して1秒間の停止をしています。

方法2：コンパレータで判定

こちらの方法ではオペアンプを使った電圧コンパレータを使い設定した電圧より低い場合にデジタル入力から読み取れるようにします。デジタルの入力で処理するので方法１のような処理速度の問題はありません。部品が増えるのが欠点ですがオペアンプと半固定抵抗器だけです。半固定抵抗で閾値を簡単にセットできるという利点もあります。

コンパレータを使ったストール検出

スケッチは次のとおりです。

const int pulsePin = 3;
const int dirPin = 2;
const int stall = 4;
const int led = 5;

int stallState = 0;
int stallCheck;


void setup() {
  pinMode(pulsePin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(stall, INPUT);
  pinMode(led, OUTPUT);

  delay(10);
  digitalWrite(pulsePin, LOW);
  digitalWrite(dirPin, LOW);
  digitalWrite(led, LOW);

  digitalWrite(led, HIGH);
  delay(500);
  digitalWrite(led, LOW);
  stallCheck = 0;
}



void loop() {
  digitalWrite(pulsePin, HIGH);
  delayMicroseconds(2);
  digitalWrite(pulsePin, LOW);
  delayMicroseconds(800);

  stallCheck++;
  if (stallCheck > 30 ) {
   stallState = digitalRead(stall);
 
    if (stallState == HIGH) {
      digitalWrite(led, HIGH);
      delay(1);
      digitalWrite(dirPin, HIGH);
      for (int i = 0 ; i <= 300 ; i++) {
        digitalWrite(pulsePin, HIGH);
        delayMicroseconds(2);
        digitalWrite(pulsePin, LOW);
        delayMicroseconds(800);
      }
      digitalWrite(dirPin, LOW);
      stallCheck = 0;
      delay(100);
    } else {
      digitalWrite(led, LOW);
    } 
  }
}

オペアンプLM358を使ったコンパレータで電圧を比較してLM358の出力をArduinoでデジタルで読み込んでるので処理は早いです。半固定で閾値を設定できるのも便利。ストール検出はモータの速度が変わると正常に機能しなくなるそうです。上記のコードではモータが回り始めた30ステップは読み取りを無効にしています。そしてストールを検出したら一定角度逆回転するようにしています。

DRV8434A単体で学習モードを使い閾値を設定して動作させることが可能らしいですが、数ステップ正常に回してから数ステップの間ストールさせて学習完了とか面倒そうなので試してません。（そしてその閾値がドライバに保存されるの？伝d源投入時に毎回学習モードにする必要あるの？？？）

最後に動作している様子です。


いいなと思ったら応援しよう！