TMC2209 アナログデバイセズのモータードライバー StallGuard/CoolStep お試し。

これまで DRV8835, DRV8825 や A4988 といった Stepper Driver は触ったことがあるのですが、これらは単純に DIR/STEP を入力して回す単純なものでした。その代わり、電流を決定するにその設定の Vref を調整すると言った手間が生じます。

最近(でもないのかもしれませんが) 3D プリンタ界隈では "センサーレスホーミング" と言った機能があるらしく、どうやら可動域の限度をリミットスイッチではなく物理的に生じる負荷抵抗をモータドライバが検知して既存のリミットスイッチを不要とする機能があるようです。

へぇ、面白そう

と思いつつ、色々調べてみると、一般的にはストール検知(stall detection)と呼ばれる機能でここ数年各メーカのモータードライバの一部が持つ機能のようでした。手持ちのモータードライバを漁ってみると、丁度 Analog Devices の TMC2209 を使った MKS TMC2209 v2.0 のモジュールがあり、これがセンサーレスホーミングに対応していたので ESP32 で制御してストール検知(StallGuard)を試してみました。

はじめに

TMC2209 には StallGuard/CoolStep と呼ばれる機能があります。3D プリンターはこの機能を使ってセンサーレスホーミングを実現しています。

StallGuard/CoolStep を利用するには UART を用いて TMC2209 を制御する必要があります。同時に UART 経由でモーターへの電流を設定することが可能で、これまでの Vref への可変抵抗を用いた調整が不要になります。

また、今回調べた範囲では UART を用いて回転方向と速度を設定してモーターを回転させることは出来るものの、ステップ数で管理した回転の制御というのは出来ないようでした。

なので

モーターの電流、 StallGuard/CoolStep の設定や利用は UART
モーターの回転の方向や速度、回転位置(ステップ数)管理は DIR/STEP

として制御するものとしました。

制御側のマイコンには ESP32 を用いていますが Arduino Framework を使っているので ESP32 に限らず利用するライブラリの互換範囲で動作すると思います。

ハードウェア

使用部品

NodeMCU-32S
MKS TMC2209 v2.0 ブレッドボードに挿すのに邪魔だったので DIAG/INDEX に取り付けられていたピンは外しました。
NEMA17 Stepper Motor (BJ42D22-23V01) 3Dプリンタの余り部品/スペック不明、抵抗値を測ったところ 3Ω程度でした
1kΩ抵抗(UART TX/RX 分離用)
ブレッドボード/配線部材

今回モーター駆動の電源は USB の 5V を使っています。(お勧めはません。自己責任で)

配線

TMC2209 の UART は一つの端子で TX/RX を兼ねています。マイコン側の UART は TX/RX それぞれあり、マイコン側の TX は 1kΩの抵抗を挟んで TMC2209 と繋ぐ必要があります。これをしないと、マイコンからデータは送信できるが TMC2209 からのデータが受信できないというトラブルになります。

MKS TMC2209 v2.0 のモジュールは2つの UART ピンがありますが、回路図を確認すると TMC2209 の UART から 0Ωの抵抗(R8)を挟んで繋がれており、別途 1kΩの抵抗が必要になります。(R8 が 0Ωなのは実装ミスなんじゃなかろうか) TMC2209 を使った別のモジュール、例えば FYSETC の Silent2209 の v2.1 以降のモジュールは 1kΩの抵抗を介した UART PIN が提供されており、そのまま繋ぐことが出来ます。利用するモジュールのマニュアルや回路図を確認の上配線してください。

TMC2209 の UART を含む信号レベルは VCC_IO (モジュールでは VDD)の入力で決定します。ESP32 の信号レベルは 3.3v なので NodeMCU の 3.3v 出力を繋ぎます。

ENBLE/DIR/STEP はこれまで一般的なモータドライバ同様、GPIO のピンに接続します。

DIAG は StallGuard の機能による出力となり、マイコン側の GPIO 入力としてピンに接続します。

接続

NodeMCU32S	MKS TMC2209 V2.0
HW Serial2 RX GPIO16	PDN-UART
HW Serial2 TX GPIO17	PDN-UART(1kΩ抵抗経由)
GPIO18	DIR
GPIO19	STEP
GPIO21	ENABLE
GPIO23	DIAG
3.3v	VDD (VCC_IO)
VIN(5v)	VM (VS)
GND	GND

実体配線

ソフトウェア

開発環境は vscode/devcontainer + PlatformIO + Arduino Framework としています。

利用ライブラリ

TMCStepper UART 経由で TMC2209 を設定するライブラリです。

github.com

FastAccelStepper STEP/DIR/ENABLE を用いてモーターの回転と位置を制御/管理するライブラリです。

github.com

以上２つのライブラリを利用します。 platformio.ini は

[env:nodemcu-32s]
platform = espressif32
board = nodemcu-32s
monitor_speed = 115200
framework = arduino
lib_deps =
  SPI
  gin66/FastAccelStepper @ ^0.30.11
  teemuatlut/TMCStepper @ ^0.7.3

となりました。

プログラミング

TMCStepper の TMC2209 のドライバのコンストラクターの引数は、Serial Port, RSense, Address になります。
- Serial Port はマイコン側のシリアルポート(今回はハードウェアシリアルとして Serial2)を指定します。
- RSense は BRA/BRB ピンから GND の間に挟まれるモジュールに実装されている抵抗(sense resister)の値を指定します。MKS TMC2209 V2.0 は回路図から 0.11Ω(0R11)が読み取れるので、0.11f を指定します。
- UART に繋がれる TMC2209 のアドレスを指定します。今回は一つしか接続せず特に設定を行っていないので 0 となります。(MS1/MS2 ピンの設定で決まります)
TMC2209 で設定した Serial2 を baudrate を指定して初期化し、TMC2209 ドライバを開始、基本的な設定を行います。
- toff を設定し TMC2209 内部のソフトウェアドライバを開始します。
- rms_current を用いてモーター電流を設定します。(今回は 900mA)
- microsteps を用いてマイクロステップを設定します。(今回は 4 を設定し 1/4 としています)
CoolStep/StallGuard を設定します。
CoolStep は負荷に対して電流を制御する機能です。CoolStep に対する設定は負荷に対する上限と下限の閾値を指定します。設定の上限を超える負荷を検知すると設定されたモーターへの供給電流の値とその 1/2 の範囲でモータへ供給する電流を増やし、負荷が下限より下がると電流を減らします。これによって消費電力を下げモータの無駄な発熱等の回避が可能になります。
StallGuard は負荷の限界を設定し、負荷がその設定より上回ると DIAG ピンに出力を行います。
それぞれの動作に対する負荷は TMC2209 が生成する SG_RESULT と呼ばれる値と比較されます。SG_RESULT は 負荷が大きくなると値が小さくなります。
- CoolStep が動作する最小速度閾値である 20bit の TCOOLTHRS を設定します。値はパルス期間のクロック数なので値が大きいほど速度は遅いという事になります。今回は動作確認なので設定可能な下限(最大値)である 0xFFFFF を設定します。
- CoolStep がモーターに供給する電流を可能であれば上げ始める負荷の閾値を設定します。閾値は semin で指定されます。semin で指定される N の N * 32 の値を負荷の値(SG_RESULT)と比較され、負荷の値が閾値より小さい(負荷が大きい)と電流を増やそうとします。
- CoolStep がモーターに供給する電流を可能であれば下げ始める負荷の閾値を設定します。閾値は semax で指定されます。semax で指定される M の (M + N(semin) + 1) * 32 の値を負荷の値(SG_RESULT)と比較され、負荷の値が閾値より大きい(負荷が小さい)と電流を減らそうとします。
- StallGuard が DIAG ピンに出力する閾値を SGTHRS で設定します。SHTHRS に与える値は閾値の半分(/ 2)の数値になります。
FastAccelSteooer を設定しモータを動作させます。
- STEP/DIR/ENABLE ピンを FastAccelStepper にアサインします。
- 自動で ENABLE ピンを制御させるために setAutoEnable を true にします。
- モーターを回転させる最大速度を setSpeedInHz を用いて周波数で設定します。
- モーダーの速度を変化させる加速度を setAcceleration を用いて設定します。
- move を用いて動作ステップ数を指定しモーターを動作させます。
出力ループ
CoolStep と StallGuard の動作を確認するため
- 負荷の値(SG_RESULT)
- モーターに供給される電流の値(cs_actual で得られる値を cs2rms で変換した値)
- StallGuard で制御される DIAG ピンの出力
- semin/semax/SGTHR で設定された各閾値
  - semin = 5: 5 * 32 = 160 (CoolStep 下限閾値)
  - semax = 2: (5 + 2 + 1) * 32 = 256 (CoolStep 上限閾値)
  - SGTHRS = 100: 100 * 2 = 200 (StallGuard 閾値)
- FastAccelStepper で設定されるモーター速度(周波数 Hz)

を出力します。出力フォーマットは vscode の Teleplot を用いてグラフ化する事として対応させます。

marketplace.visualstudio.com

実装

#include <Arduino.h>

#include "TMCStepper.h"
#include "FastAccelStepper.h"

#define dirPinStepper 18
#define stepPinStepper 19
#define enablePinStepper 21
#define diagPin 23

FastAccelStepperEngine engine = FastAccelStepperEngine();
FastAccelStepper *stepper = NULL;

TMC2209Stepper driver(&Serial2, 0.11f, 0); // RSense 0.11Ω, UART Address 0

void setup() 
{
    pinMode(diagPin, INPUT);
    Serial.begin(115200);

    constexpr uint8_t us = 1 << 2;
    Serial2.begin(115200);          // HW UART Serial2 (RX = GPIO16, TX = GPIO17)
    driver.begin();
    driver.toff(5);                 // Enables driver in software
    driver.rms_current(900);        // motor current mA
    driver.microsteps(us);

    driver.TCOOLTHRS(0xFFFFF);
    driver.semin(5);
    driver.semax(2);
    driver.SGTHRS(100);

    engine.init();
    stepper = engine.stepperConnectToPin(stepPinStepper);
    if (stepper) {
        stepper->setDirectionPin(dirPinStepper);
        stepper->setEnablePin(enablePinStepper);

        stepper->setAutoEnable(true);

        stepper->setSpeedInHz(250 * us);
        stepper->setAcceleration(25 * us);
        stepper->move(4096 * us ) ;
    }
}

void loop() 
{
    delay(100);
    Serial.printf(">SG_RESULT:%d\n", driver.SG_RESULT());
    Serial.printf(">motor current(mA):%d\n", driver.cs2rms(driver.cs_actual()));
    Serial.printf(">Speed(Hz):%ld\n", stepper->getCurrentSpeedInMilliHz() / 1000);
    Serial.printf(">DIAG Pin:%d\n", digitalRead(diagPin) * 100);
    Serial.printf(">CoolStep lower threshold:%d\n", 5 * 32);
    Serial.printf(">CoolStep upper threshold:%d\n", (5 + 2 + 1) * 32);
    Serial.printf(">StallGuard threshold:%d\n", 100 * 2);
}

実行結果

まずそのまま実行し Telepolot を用いて出力からグラフを描画します。

グラフは時間経過を X 軸として Y 軸はそれぞれ

黄色: Speed(Hz), モーターの回転速度としての STEP に与えられるパルスの周波数
緑色: motor current(mA), CoolStep によって制御されるモーターへの供給電流(mA)
橙色: SG_RESULT, TMC2209 で検出される負荷の値
青色: CoolStep upper threshold, semax で設定される CoolStep の負荷上限閾値(=256 固定値)
赤色: StallGuard threshold, semin で設定される DIAG ピンへの出力を制御する StallGuard の閾値(=200 固定値)
黄緑色: CoolStep lower threshold, semin で設定される CoolStep の負荷下限閾値(=160 固定値)
紫色: Diag Pin, StallGuard で制御される DIAG ピン出力

になります。

CoolStep

橙色の SG_RESULT と、青色の CoolStep 上限閾値、黄緑色の CoolStep 下限閾値、それらに対する結果として緑色のモーターへの供給電流を見ます。

モーターが停止状態から動作を開始し加速を始め SG_RESULT が CoolStep 下限閾値未満の高負荷時は設定された供給電流値まで増加させています。
加速が続きモーターがある程度の速度になり負荷の値である SG_RESULT が CoolStep 上限閾値以上の低負荷になるとモーターへの供給電流が下がり始め、供給電流の下限値である 1/2 まで下がります。
動作が続いて指定された動作ステップ数に近づき、減速が開始されると再び負荷が上がり始め SG_RESULT の値が下がり始めます。
減速が続き、負荷である SG_RESULT の値がが CoolStep 下限閾値未満の高負荷状態となり再び供給電流を増加させます。

といった、加速と減速の負荷の変動とその負荷の各閾値に対する供給電流の制御という CoolStep の挙動が確認できます。