見出し画像

ESP32とL6470でモータを回してみた⑤【RunコマンドとMoveコマンドの違い】

オラット

ESP32とL6470を使ってステッピングモータを回したシリーズの記事が増えてきたので「もくじページ」を作成しました。→もくじページのリンク

前回までの振り返り

自作のESP32基板とL6470使用 ステッピングモータドライブキット（以後L6470）を使ったステッピングモータの回転に成功し、マイクロステップやKVALの設定についてはオシロスコープをなんとか使いイメージできました。
前回は、これから使うであろうL6470からのレスポンスが取得できる究極の関数（？）をGeminiくんとチャッピーが完成させました。

今回やったこと

  • これまでモータを回すコマンドとしてRunコマンドを使用してきましたが、このコマンドは連続的に回転するだけで、モータを1周だけ回すとか、90°だけ回すなどの座標制御には対応していません。そこで、パルスの数で移動量を指定できるMoveコマンドが使えるのではないかと思い調べてみました。

  • 結論からいうと、一定速度で回す場合はRunコマンドを使用し、決まった位置に停止させたい場合はMoveコマンドが使えそうです。

ただし、どちらのコマンドも回転速度の設定にはクセがあることがわかりました。

ESP32基板は、自分で設計しJLCPCBで製作してもらったESP32を使用しています。もちろん他から購入しても同じです。

Runコマンドについて

L6470のデータシートの60ページにRunコマンドについての記載があります。
＜デバイスメーカーのデータシートはこちら＞

英語の説明を翻訳したのがこちらです。

Runコマンドは、SPDで指定された速度で動作を生成します。方向はDIRビットによって選択されます（'1'で正転、'0'で逆転）。SPDの値は step/tick（符号なし固定小数点 0.28 フォーマット）で表現され、これはSPEEDレジスタ（42ページのセクション9.1.4を参照）と同じフォーマットです。
注：
SPDの値はMAX_SPEEDより小さく、MIN_SPEEDより大きい必要があります。そうでない場合、RunコマンドはそれぞれMAX_SPEEDまたはMIN_SPEEDで実行されます。
このコマンドは、目標速度（target speed）に達するまでBUSYフラグをロー（Low）に維持します。
このコマンドはいつでも発行することができ、即座に実行されます。

なるほど「 SPDの値はstep/tick（符号なし固定小数点 0.28 フォーマット）で表現され．．．」全く理解できません。
42ページのセクション9.1.4を参照しろと書いてあります。

翻訳するとこうです。

SPEEDレジスタには、現在のモーター速度が格納されており、step/tick（符号なし固定小数点 0.28 フォーマット）で表現されます。SPEEDレジスタの値を step/s（1秒あたりのステップ数）に変換するには、以下の数式を使用できます。
数式４：[step/s] ＝ (SPEED × 2^-28) ÷ tick
ここで、SPEED はレジスタに格納されている整数値であり、tick は 250ns です。設定可能な範囲は 0 から15625 step/s で、分解能は 0.015 step/s です。
注：
ユーザーが実際に利用できる範囲は、MAX_SPEEDパラメータによって制限されます。このレジスタに（直接）書き込みを行おうとすると、そのコマンドは無視され、NOTPERF_CMDフラグがセット（Highに）されます（55ページのセクション9.1.22を参照）。

では、私が使用している0.9°のモータで1回転/s（ = 1rps = 60rpm ）で回転させる場合の計算をします。
モーターが1回転するのに必要なステップ数：360°/0.9°＝400step/s・・・①

数式４から
[step/s] ＝ (SPEED × 2^ (-28) ) ÷ tick
SPEED=[step/s] × tick × 2＾28
tickは250nsだから250 × 10 ^ (-9)。　2＾28は268,435,456なので
SPEED=[step/s] × 67.108864
この式に①を入れると
SPEED = 400 × 67.108864 = 26843.5456 ≒ 26844
これを16進数に変換すると”0x0068DC”となります。

各回転数と設定値の表はこのようになります。

では、実際に確認してみます。プログラムはこちらです。

#include <Arduino.h>
#include <SPI.h>

#define PIN_SS 5

// 【モーター1個用・究極完全版】他デバイス共存・マルチタスク安全・ガードレール付き
template <typename... Args>
uint32_t L6470_send(Args... args)
{
    // 4バイトを超える引数を入れたらコンパイルエラーとする
    static_assert(sizeof...(args) <= 4, "L6470_send supports up to 4 bytes");

    // SPI設定をその都度安全に適用（他デバイスとの競合や設定ズレを防ぐ）
    SPI.beginTransaction(SPISettings(1000000, MSBFIRST, SPI_MODE3));

    uint8_t tx[] = {static_cast<uint8_t>(args)...};
    uint32_t rx_data = 0;

    // 送りたいバイト数の分だけ、データを送受信
    for (size_t i = 0; i < sizeof(tx); ++i)
    {
        digitalWrite(PIN_SS, LOW);
        uint8_t res = SPI.transfer(tx[i]);
        digitalWrite(PIN_SS, HIGH);

        // 返ってきた生のレスポンスをそのまま32bitに詰め込む
        rx_data = (rx_data << 8) | res;

        delayMicroseconds(1);
    }

    // 通信終了、SPIバスを他の処理に解放
    SPI.endTransaction();

    return rx_data;
}

void setup()
{
    Serial.begin(115200);

    delay(1000);

    Serial.println("=== ESP32 Program Info ===");
    Serial.println("Program: 260531_L6470_run_test.cpp");
    Serial.print("Build Date: ");
    Serial.print(__DATE__);
    Serial.print(" ");
    Serial.println(__TIME__);
    Serial.println("==========================");

    pinMode(PIN_SS, OUTPUT);
    digitalWrite(PIN_SS, HIGH);

    // SPIの開始
    SPI.begin();
    delay(200);

    // デバイスリセット
    L6470_send(0xC0);
    delay(10);

    // 【重要】リセット直後のStatus空読み
    L6470_send(0xD0, 0x00, 0x00);
    delay(10);

    // STEP_MODEレジスタ(0x16)のBit7(SYNC_EN)を1に、Bit6-4を001にする
    L6470_send(0x16, 0x97); // デフォルトのフルステップ(0x07)にSYNC_EN(0x80)を足した場合の例
}

void loop()
{
    // 5秒間、正転
    L6470_send(0x51, 0x00, 0x68, 0xDC);
    delay(5000);

    // 回転中のステータスを取得してシリアルに表示
    uint32_t r = L6470_send(0xD0, 0x00, 0x00);
    uint16_t status = r & 0xFFFF; // 下位16bit（ステータス本体）を抽出

    Serial.print("L6470 Status: 0x");
    Serial.println(status, HEX);

    // 1秒間、停止（SoftStopコマンド 0xB0）
    L6470_send(0xB0);
    delay(1000);
}

SYNC出力でパルスの周波数を確認したいので、68行目にSTEP_MODEレジスタに設定を入れました。設定内容は1パルスごとにSYNCの信号を出力するものです。この設定はマイクロステップの細かさも設定できる箇所ですが、こちらはデフォルトと同じ1/128としています。（割愛しますがデータシート47ページのセクション9.1.19参照）

L6470_send(0x16, 0x97);

回転速度は上で計算した”0x0068DC”です。回転時間は５秒としましたので5周回ったら停止する設定です。

 L6470_send(0x51, 0x00, 0x68, 0xDC); 
    delay(5000);

   実際に回転しているところです。gif動画なのでズレが生じていると思いますが、現物はきっちり5秒間回転しています。しかし、1周を１秒間で回転するように設定したのに、きっちり5秒の回転でスタートの位置と同じ場所に停止していません。

SYNC出力をオシロスコープで取得したのが下の画像です。計算は400Hz狙いでしたが、409.8Hzと少しズレています。これはL6470の内部オシレータ（内蔵発振器）の精度もしくは分解能が原因だと思います。このため想定していた停止位置より毎回オーバーして停止すると考えられます。

Moveコマンドについて

続いてMoveコマンドでモータを回してみます。
L6470のデータシートの60ページにMoveコマンドについての記載があります。
＜デバイスメーカーのデータシートはこちら＞

英語の説明を翻訳したのがこちらです。

Moveコマンドは、N_STEPで指定されたマイクロステップ分の動作を生成します。方向はDIRビットによって選択されます（'1'で正転、'0'で逆転）。
N_STEPの値は、常に選択されているステップモード（ステップ分解能）と一致します。つまり、パラメータ値の単位は、選択されているステップモード（フル、ハーフ、クォーターなど）と等しくなります。
このコマンドは、目標のステップ数が実行（完了）されるまでBUSYフラグをロー（Low）に維持します。このコマンドは、モーターが停止しているときにのみ実行できます。もし動作中の場合は、一度モーターを停止させることでMoveコマンドを実行できるようになります。
モーターが回転している（動作している）ときにMoveコマンドを実行しようとすると、そのコマンドは無視され、NOTPERF_CMDフラグがセット（Highに）されます（55ページのセクション9.1.22を参照）。

MoveコマンドはRunコマンドとは違い指定した座標に止めることができそうです。なお、座標の移動量を計算するためには、ステップモードの設定値を意識する必要があります。
私の条件で１周分のステップを計算する場合は360° ÷ 0.9° × 128（マイクロステップの設定）= 51,200となります。
16進数に変換すると”0x00C800”です。
私は５周回転させたいので5倍の128,000ステップとなり、16進数に変換すると”0x03E800”となります。

この部分戸惑ったのですがMoveコマンドは速度のパラメータがないので、なんの設定も行わずに動作させるとMAX_SPEEDで動いてしまいます。このためMoveコマンドを使う場合はMAX_SPEEDレジスタで速度を規制する必要があります。

MAX_SPEEDをデータシート（43ページ）で確認してびっくりしたのですが、RunコマンドのSPEEDを計算する式の乗数部分が違いました。同じだと思って設定をミスしてしまうと大変なことになりますね！

MIN_SPEEDも違うようなので、以下にまとめてみました。

では、私が使用している0.9°のモータを1回転/s（ = 1rps = 60rpm ）で回転させる場合の計算をします。こちらはマイクロステップは計算に出てきません。
モーターが1回転するのに必要なステップ数：360°/0.9°＝400step/s・・・①

数式7から
[step/s] ＝ (MAX_SPEED × 2 ^ (-18) ) ÷ tick
MAX_SPEED=[step/s] × tick × 2＾18
tickは250nsだから250 × 10 ^ (-9)。　2＾18は262,144なので
MAX_SPEED=[step/s] × 0.065536
この式に①を入れると
MAX_SPEED = 400 × 0.065536 = 26.21 ≒ 26
これを16進数に変換すると”0x001A”となります。

各回転数と設定値の表はこのようになります。

では、実際に確認してみます。プログラムはこちらです。

#include <Arduino.h>
#include <SPI.h>

#define PIN_SS 5

// 【モーター1個用・究極完全版】他デバイス共存・マルチタスク安全・ガードレール付き
template <typename... Args>
uint32_t L6470_send(Args... args)
{
    // 4バイトを超える引数を入れたらコンパイルエラーとする
    static_assert(sizeof...(args) <= 4, "L6470_send supports up to 4 bytes");

    // SPI設定をその都度安全に適用（他デバイスとの競合や設定ズレを防ぐ）
    SPI.beginTransaction(SPISettings(1000000, MSBFIRST, SPI_MODE3));

    uint8_t tx[] = {static_cast<uint8_t>(args)...};
    uint32_t rx_data = 0;

    // 送りたいバイト数の分だけ、データを送受信
    for (size_t i = 0; i < sizeof(tx); ++i)
    {
        digitalWrite(PIN_SS, LOW);
        uint8_t res = SPI.transfer(tx[i]);
        digitalWrite(PIN_SS, HIGH);

        // 返ってきた生のレスポンスをそのまま32bitに詰め込む
        rx_data = (rx_data << 8) | res;

        delayMicroseconds(1);
    }

    // 通信終了、SPIバスを他の処理に解放
    SPI.endTransaction();

    return rx_data;
}

void setup()
{
    Serial.begin(115200);

    delay(1000);

    Serial.println("=== ESP32 Program Info ===");
    Serial.println("Program: 260531_L6470_move_test.cpp");
    Serial.print("Build Date: ");
    Serial.print(__DATE__);
    Serial.print(" ");
    Serial.println(__TIME__);
    Serial.println("==========================");

    pinMode(PIN_SS, OUTPUT);
    digitalWrite(PIN_SS, HIGH);

    // SPIの開始
    SPI.begin();
    delay(200);

    // デバイスリセット
    L6470_send(0xC0);
    delay(10);

    // 【重要】リセット直後のStatus空読み
    L6470_send(0xD0, 0x00, 0x00);
    delay(10);

    // STEP_MODEレジスタ(0x16)のBit7(SYNC_EN)を1に、Bit6-4を001にする
    L6470_send(0x16, 0x97); // デフォルトのフルステップ(0x07)にSYNC_EN(0x80)を足した場合の例

    // MAX_SPEEDを400 step/sに制限する正しい書き方
    L6470_send(0x07, 0x00, 0x1A);
}

void loop()
{
    // 5回転分、正転
    L6470_send(0x41, 0x03, 0xE8, 0x00);

    // 完全に停止（MOT_STATUS == 00）するまでループで待機
    uint16_t status = 0;
    uint16_t mot_status = 0;

    do
    {
        // 回転中のステータスを取得（GetStatus: 0xD0）
        uint32_t r = L6470_send(0xD0, 0x00, 0x00);
        status = r & 0xFFFF; // 下位16bit（ステータス本体）を抽出

        // Bit6-5の「MOT_STATUS」だけを抽出して右に5シフト
        // (status & 0x0060) でBit6-5以外をマスクし、>> 5 で 0〜3 の値にする
        mot_status = (status & 0x0060) >> 5;

        Serial.print("L6470 Status: 0x");
        Serial.print(status, HEX);
        Serial.print(" | MOT_STATUS: ");
        Serial.println(mot_status); // 0:停止, 1:加速, 2:減速, 3:定速

        // 10ms待機（シリアル溢れとSPI負荷の防止）
        delay(10);

    } while (mot_status != 0); // MOT_STATUSが 0（Stopped）以外の一時、ループを継続

    // 1秒間、停止（SoftStopコマンド 0xB0）
    // L6470_send(0xB0);
    delay(1000);
}

Runコマンドと同様にSYNC出力周波数を確認するためのSTEP_MODEレジスタを65行目に入れています。それに加えて、上で計算したMAX_SPEEDを入力しています。

L6470_send(0x07, 0x00, 0x1A);

void loop内に５回転分のステップ数”0x03E800”（10進数で51,200ステップ）

L6470_send(0x41, 0x03, 0xE8, 0x00);

あと、気をつけないといけないのがRunコマンドの場合はコマンド直後の”delay(5000);”でプログラムを止めていますが、Moveコマンドの場合はデータを送信した後にもプログラムが進んでしまいます。そこで、回転中のステータスを取得し、モータが回っている間はプログラムが進まないように下のループをプログラムに追加しています。

// 完全に停止（MOT_STATUS == 00）するまでループで待機
    uint16_t status = 0;
    uint16_t mot_status = 0;

    do
    {
        // 回転中のステータスを取得（GetStatus: 0xD0）
        uint32_t r = L6470_send(0xD0, 0x00, 0x00);
        status = r & 0xFFFF; // 下位16bit（ステータス本体）を抽出

        // Bit6-5の「MOT_STATUS」だけを抽出して右に5シフト
        // (status & 0x0060) でBit6-5以外をマスクし、>> 5 で 0〜3 の値にする
        mot_status = (status & 0x0060) >> 5;

        Serial.print("L6470 Status: 0x");
        Serial.print(status, HEX);
        Serial.print(" | MOT_STATUS: ");
        Serial.println(mot_status); // 0:停止, 1:加速, 2:減速, 3:定速

        // 10ms待機（シリアル溢れとSPI負荷の防止）
        delay(10);

    } while (mot_status != 0); // MOT_STATUSが 0（Stopped）以外の一時、ループを継続

実際に回転しているところです。5周回転した後にピッタリとスタートした位置に停止しています。

SYNC出力をオシロスコープでみたのが下の画像です。Runコマンドと同様にL6470の内部オシレータ（内蔵発振器）の精度や分解能が原因で狙っていた400Hzから数％ズレています。ただ、Moveコマンドの移動量はパルスの数で制御するのでモータの停止は狙い通りにすることができました。

所感

今回実験し気がついたことを以下記載しますが、まだ良く分かっていないことがあります。使いこなすためには継続して勉強する必要がありそうです。

  • RunコマンドとMoveコマンドを使い、ほぼ狙い通りの回転速度でステッピングモータを回わすことができた。

  • 回転数1rpsは400Hz（0.9°/stepのモータの場合）であるが、オシロスコープで周波数を調べた結果、RunとMoveのどちらも409.8Hzとなり、内部オシレータが影響しぴったり400Hzにはならない？

  • Runの回転速度はコマンドのパラメータで設定でき、設定可能囲は 0 ～15625 step/s、分解能は 0.015 step/sであるのに対し、MoveはMAX_SPEEDで回転速度を設定する。設定可能囲は15.25 ～15625 step/s、分解能は 15.25 step/sと粗い。Moveは現在の座標から決まった座標まで動かすのが目的のコマンドで、速度を厳密に制御しながら回すのには向いていない？

  • Runは一定速度で連続的に回転させるのには向いているが設定した座標で止めるのは難しい。（GetParamで座標を取得し停止する？）

  • Moveコマンドは速度設定の分解能が小さいことから退避動作とか、３Dプリンターだと造形していないヘッドの移動などを想定した動作のような気がします。


非常に長い投稿となってしまいましたが、最後まで読んでいただきありがとうございました。





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