これまで 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 を設定するライブラリです。
- FastAccelStepper STEP/DIR/ENABLE を用いてモーターの回転と位置を制御/管理するライブラリです。
以上２つのライブラリを利用します。 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 になります。
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 の動作を確認するため
を出力します。出力フォーマットは vscode の Teleplot を用いてグラフ化する事として対応させます。
実装
#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 の挙動が確認できます。
StallGuard
橙色の SG_RESULT と、赤色の StallGuard 閾値、それらに対する結果として 紫色の DIAG ピン出力を見ます。
動作は非常に単純で、負荷の値である SG_RESULT が閾値未満の高負荷状態の時、DIAG ピンに出力されるというだけの挙動になります。グラフもその挙動を示しています。
外部から負荷を与えた時の動作
次に加速/減速の負荷ではなく、等速動作中に外部から負荷を与えた時の挙動を確認してみます。値は 0.1 秒単位で取得しているので瞬間的に下回る/上回るといった状態の観測にやや難がありますが‥
- 負荷を与えると SG_RESULT の値が下がります。
- SG_RESULT の値がCoolStep の負荷上限閾値以上の低負荷になると供給電流は設定電流値の半分まで減り続けます
- SG_RESULT の値がStallGuard の負荷閾値未満の高負荷になると DIAG ピンに出力されます
- SG_RESULT の値がCoolStep の負荷上限閾値以上の低負荷になると供給電流は設定電流値の半分まで減り続けます
という StallGuard/CoolStep の挙動が確認できます。
