Feetech サーボの値を読み取る

Feetechサーボとお話しする

やること

前回と前々回の続きです。
FeetechのSTS3215について、書き込み方法や情報のインデックスなどがわかってきました。
そこで今度はESP32マイコンでサーボの情報を読み取ってみます。

作業環境

• Arduino IDE
• ESP32DevkitC
• STS3215, SCS0009
• 半二重回路

半二重回路はどれでもよいですが、今回は「Meridian Board - Lite -」を使用しています。
SCS0009についてはまだしっかりと調べられていませんが、おそらくほぼ同じコマンドで動くと思いますが、メモリ構造でSTS3215と違う箇所があるようです。
特に2バイトのデータについて、SCS0009ではリトルエンディアンとビックエンディアンが逆になっておりますので、コードも一部書き換えて使用してください。

参考

↓前回記事。コマンドのメモリテーブルについてのまとめ。

↓前々回記事。サーボにコマンドを送って動かす方法。

↓秋月のサイトでDLできる公式資料は便利です。

STS3215, SCS0009とESP32の接続

今回は半二重回路経由で接続します。
ENピンやハードウェアシリアルについてはご自身の環境に読み替えて進めてください。

またこれはテスト用の簡易接続であるため。サーボを正しく動かしたい場合には、別途サーボに電源供給を行ってください。

サーボ線	機能	半二重 回路(外)	半二重 回路(内)	ESP32
外側の黒線	GND		GND	GNDピン
中央の黒線	Vcc		Vcc	5Vピン
外側の白線	信号線	入出力ピン	RX	16番ピン(Serial2 RX)
			TX	17番ピン(Serial2 TX)
			EN	4番ピン

サンプルコード

ESP32_FeetechReader.ino

#define ENPIN           4    // 半二重回路のENピン
#define STS_TIMEOUT     800  // 応答待ち時間（マイクロ秒）
#define MAX_DATA_LENGTH 64   // 最大データ長（適宜変更してください）
byte buffer[MAX_DATA_LENGTH];// 受信データを格納するバッファ

// 値をDEC(HEX)形式でシリアルモニタに表示
void disp_dechex(int num) {
  Serial.print(num, DEC); // HEX形式で表示
  Serial.print(" (0x");
  if (num < 0x1000) Serial.print("0"); // 3桁の場合、先頭に0を追加
  if (num < 0x100) Serial.print("0");  // 2桁の場合、先頭に0を追加
  if (num < 0x10) Serial.print("0");   // 1桁の場合、先頭に0を追加
  Serial.print(num, HEX); // HEX形式で表示
  Serial.print(")");
}

// 指定された文字が数字かどうかを判定
bool isDigit(char ch) {
  return (ch >= '0' && ch <= '9');
}

// 文字列が半角数字のみかどうかをチェック
bool isNumeric(String str) {
  for (unsigned int i = 0; i < str.length(); i++) {
    if (!isDigit(str.charAt(i))) {
      return false;
    }
  }
  return true;
}

// PCからシリアル入力された値を読み取って返す
int readNumber(int order, int minimum, int maximum) {
  while (true) {
    while (!Serial.available()) {
      // シリアル入力を待機
    }
    String inputData = Serial.readStringUntil('\n');
    inputData.trim(); // 改行や余計な空白を削除
    if (isNumeric(inputData)) {
      int number = inputData.toInt();
      // 数字が指定された範囲内にあるかチェック
      if (number >= minimum && number <= maximum) {
        return number;
      } else {
        Serial.print("Enter a number between ");
        Serial.print(minimum);
        Serial.print(" and ");
        Serial.print(maximum);
        Serial.println(": ");
      }
    } else {
      Serial.print("Invalid input: ");
    }
  }
}

// チェックサムの計算
byte sts_calcCkSum(byte arr[], int len) {
  int checksum = 0;
  for (int i = 2; i < len - 1; i++) {
    checksum += arr[i];
  }
  return ~((byte)(checksum & 0xFF)); // チェックサム
}

// コマンドをサーボに送信する
void sts_sendMsgs(byte arr[], int len) {
  digitalWrite(ENPIN, HIGH);// ENピンをアクティブにして送信モードにする
  for (int i = 0; i < len; i++) {// コマンドパケットを送信
    Serial2.write(arr[i]);
  }
  Serial2.flush();// シリアルバッファの送信完了待ち
  digitalWrite(ENPIN, LOW);// ENピンを非アクティブにして受信モードに戻す
}

// 受信データの表示
void dispResvData(byte *buffer, int byteCount, int len) {
  Serial.print("Rsvd : ");
  for (int i = 0; i < byteCount + len - 2; i++) {
    Serial.print(buffer[i], HEX);
    Serial.print(" ");
  }
  Serial.println();
  Serial.print("Value: ");
  if (len == 1) {
    Serial.print(buffer[5], DEC);
  }
  if (len == 2) {
    Serial.print(int(buffer[6]) * 256 + int(buffer[5]), DEC);
    //SCS0009等の場合は上記の代わりに下記を使う
    //Serial.print(int(buffer[5]) * 256 + int(buffer[6]), DEC);

  }
  Serial.println();
}

// サーボデータの受信
bool receiveServoData(HardwareSerial *serialPort, byte * buffer, int byteCount, int len, unsigned long timeout) {
  int receivedBytes = 0;
  unsigned long startTime = micros();

  // 指定されたバイト数が来るまで待つ
  while (receivedBytes < byteCount + len - 2) {
    if (serialPort->available()) {
      buffer[receivedBytes] = (byte)serialPort->read(); // 指定されたシリアルポートを使用
      receivedBytes++;
    }
    // タイムアウトチェック
    if (micros() - startTime > timeout) {
      dispTimeout(timeout);
      return false; // タイムアウト
    }
  }
  dispResvData(buffer, byteCount, len);
  return true;
}

void sts_readByte(byte id, byte index, byte len) {
  byte message[8];   // コマンドパケットを作成
  message[0] = 0xFF; // ヘッダ
  message[1] = 0xFF; // ヘッダ
  message[2] = id;   // サーボID
  message[3] = 4;    // パケットデータ長
  message[4] = 2;    // コマンド
  message[5] = index;   // レジスタ先頭番号
  message[6] = len;    // 読み込みバイト数
  message[7] = sts_calcCkSum(message, 8) ; // チェックサム
  sts_sendMsgs(message, 8); // データを送信
}

// タイムアウト時の表示
void dispTimeout(int tmp) {
  Serial.print("Rsvd: TIMEOUT (over");
  Serial.print(tmp);
  Serial.println("us)");
}


void setup() {
  pinMode(ENPIN, OUTPUT);  // ENピンを出力として設定
  digitalWrite(ENPIN, LOW); // 初期状態は非アクティブ（受信モード）
  Serial.begin(115200); // シリアルモニタの設定
  while (!Serial) {
    // Serialポートが利用可能になるまで待機
  }

  Serial2.begin(1000000);  // サーボ通信用として1000000bpsに設定）
  while (!Serial2) {
    // Serialポートが利用可能になるまで待機
  }
  delay(10); // サーボの初期化待ち
}

void loop() {
  Serial.println();
  Serial.print("Servo ID(1-255)    : ");
  while (!Serial.available()) {
  }
  if (Serial.available() > 0) {
    // シリアルバッファからデータを読み取る

    // idを入力
    int id = readNumber(1, 1, 255);
    disp_dechex(id);
    Serial.println();

    // Indexを入力
    Serial.print("Index(1-64)        : ");
    int index = readNumber(3, 1, 64);
    disp_dechex(index);
    Serial.println();

    // データ長を入力
    Serial.print("Length(1-2)        : " );
    int len = readNumber(5, 1, 2);
    disp_dechex(len);
    Serial.println();

    // コマンドを送信
    sts_readByte(id, index, len);
    receiveServoData(&Serial2, buffer, 8, len, STS_TIMEOUT); // サーボからのデータを受信
  }
  delay(100);
}

つかいかた・実行結果

ESP32にスクリプトを書き込んでサーボも接続した後、シリアルモニタを開き、ESP32を一度リセットします。
「Servo ID(1-255):」の表示が出たら、
上のテキストボックスに数字を入れて「送信」を押して使います。
入力できる値は、
Servo ID(1-255): サーボのID
Index(1-64) : 読みたいデータのインデックス（表はこちら）
Length(1-2) : 読み込むデータの長さ（1バイトか2バイトか）
です。

例１　現在のサーボ位置を読み取る

Servo ID(1-255): 1
Index(1-64) : 56
Length(1-2) : 2
※STSとSCSでは使う数値の範囲が違うようです。

例２　現在のロックフラグを調べる

Servo ID(1-255): 1
Index(1-64) : 48
Length(1-2) : 1

しくみ

PCからマイコンに対してServo ID, Index, Lengthの３つのデータをシリアル経由で送ります。
３つのデータを受け取ったマイコンは、サーボに対してコマンドをシリアル２経由で送信し、すぐに返信を待つモードに入ります。
返信が来たらデータを数値に変換してシリアルモニタに表示します。

さいごに

コマンドのインデックスを参照しながらいろいろと試してみてください。
動かす命令を送信してから状態データの受信コマンドを送受信すれば動かしながらデータを見るということもできるのではないかと思います。

間違いがありましたらぜひご指摘ください。

よき電子工作ライフを〜

前回記事：

前々回記事：