0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@devgamesan

ESP32-S3でFEETECH STS3215を複数台同期制御する

0
Posted at

1. はじめに

ESP32-S3から複数のシリアルバスサーボを制御したかったため、FEETECHのSTS3215と「シリアルバスサーボドライバーボード for XIAO」を使って動作確認を行った。本記事では、ESP32-S3とサーボドライバーボードの配線、PlatformIOでのライブラリ導入、STS3215のID変更、4台のサーボを同期制御するところまでをまとめた。

2. 動作確認環境

本記事では、以下の環境で動作確認を行った。

項目 内容
OS Kubuntu 24.04.4
開発環境 Visual Studio Code + PlatformIO
Framework Arduino
マイコンボード Freenove ESP32-S3 WROOM
UART Serial1
RX GPIO18
TX GPIO17
通信速度 1 Mbps

3. 使用する部品

今回の動作確認では、以下の部品を使用した。

部品 用途 個数 購入先
Freenove ESP32-S3 WROOM サーボ制御を行うマイコンボード 1 Amazon
FEETECH STS3215 7.4V 19kg・cm 1:345 3線式シリアル通信に対応したバスサーボ 4 秋月電子
シリアルバスサーボドライバーボード for XIAO ESP32-S3のUARTとSTS3215のシリアルバス間を接続するためのドライバーボード 1 秋月電子

STS3215はデイジーチェーン接続に対応しているため、複数台のサーボを1系統のシリアルバスで制御できる。今回は4台のSTS3215を接続し、それぞれに異なるIDを設定した上で、ESP32-S3から同時に制御する。

4. シリアルバスサーボドライバの配線

ESP32-S3と「シリアルバスサーボドライバーボード for XIAO」は、以下のように接続した。

ESP32-S3 ドライバーボード
GPIO18（RX） RX
GPIO17（TX） TX

配線図は以下の通り。

image.png

一般的なUART接続では、送信側のTXと受信側のRXを交差させて接続するため、当初は以下のように配線した。

  • ESP32 RX（GPIO18） → Board TX
  • ESP32 TX（GPIO17） → Board RX
    しかし、この配線では通信できなかった。
    一方、以下のように同名端子同士を接続すると正常に動作した。
  • ESP32 RX（GPIO18） → Board RX
  • ESP32 TX（GPIO17） → Board TX
    原因は特定できていないが、少なくとも今回使用したボードと構成では、RX-RX、TX-TXの接続で動作した。

謎...

5. 依存ライブラリの導入

「シリアルバスサーボドライバーボード for XIAO」のWikiを確認すると、FEETECH製シリアルバスサーボの制御にはSCServoライブラリを利用できる。

今回は以下のライブラリを使用した。

PlatformIOでは、platformio.iniのlib_depsにGitHubリポジトリを指定することで依存ライブラリとして導入できる。

platformio.ini
[env:freenove_esp32_s3_wroom]
platform = espressif32
board = freenove_esp32_s3_wroom
framework = arduino

monitor_speed = 115200
upload_speed = 921600

build_flags =
    -D ARDUINO_USB_CDC_ON_BOOT=1
    -D ARDUINO_USB_MODE=1

lib_deps =
    https://github.com/workloads/scservo.git

これで、コードから以下のようにSCServo.hをインクルードし、STS3215を制御できるようになる。

#include <SCServo.h>

以降では、このライブラリを使用してサーボのID変更と複数台の同期制御を行う。

6. サーボのID設定

STS3215は、出荷時にはすべて同じIDが設定されている。
複数台のサーボを同じシリアルバス上で個別に制御するには、それぞれ異なるIDを割り当てる必要がある。そのため、今回は4台のサーボに1〜4のIDを設定する。
IDが重複した状態で複数台を同時に接続すると、意図したサーボだけを指定して設定できないため、ID変更はサーボを1台ずつ接続した状態で行う。
今回は、以下のコードを使用してIDを変更した。

main.cpp
#include <Arduino.h>
#include <SCServo.h>

constexpr int RX_PIN = 18;
constexpr int TX_PIN = 17;
constexpr int BAUD_RATE = 1000000;

constexpr uint8_t OLD_ID = 1;
// 変更後のID
constexpr uint8_t NEW_ID = 4;

SMS_STS servo;

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

  Serial1.begin(
    BAUD_RATE,
    SERIAL_8N1,
    RX_PIN,
    TX_PIN
  );

  servo.pSerial = &Serial1;

  delay(5000);

  // EEPROMのロック解除
  servo.unLockEprom(OLD_ID);
  delay(100);

  // サーボIDを書き換え
  servo.writeByte(OLD_ID, 0x05, NEW_ID);
  delay(100);

  // EEPROMを再ロック
  servo.LockEprom(NEW_ID);

  Serial.printf("Servo ID changed: %u -> %u\n", OLD_ID, NEW_ID);
}

void loop() {
}

このコードのNEW_IDを変更しながら1台ずつ実行し、4台のサーボにID 1〜4を設定した。

7. 複数のサーボを同期制御する

各サーボに異なるIDを設定できたので、次に4台のSTS3215を同じシリアルバスへ接続し、同期制御を行う。

今回はSCServoライブラリのSyncWritePosEx()を使用し、4台のサーボへ同時に位置・速度・加速度を指定する。

以下のコードでは、起動時に各サーボの現在位置を取得し、その位置から約1000ステップ離れた位置を目標位置として設定する。その後、2秒間隔で開始位置と目標位置を往復させる。

main.cpp
#include <Arduino.h>
#include <SCServo.h>

constexpr int RX_PIN = 18;
constexpr int TX_PIN = 17;
constexpr int BAUD_RATE = 1000000;

constexpr uint8_t SERVO_COUNT = 4;
uint8_t servoIds[SERVO_COUNT] = {1, 2, 3, 4};

constexpr int MOVE_STEPS = 1000;
constexpr uint16_t MOVE_SPEED = 100;
constexpr uint8_t MOVE_ACCEL = 10;
constexpr uint32_t MOVE_INTERVAL = 2000;

SMS_STS servo;

int startPos[SERVO_COUNT];
int targetPos[SERVO_COUNT];

bool moveToTarget = true;
uint32_t lastMove = 0;

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

  Serial1.begin(
    BAUD_RATE,
    SERIAL_8N1,
    RX_PIN,
    TX_PIN
  );

  servo.pSerial = &Serial1;

  delay(1000);

  // 各サーボの現在位置を取得
  for (int i = 0; i < SERVO_COUNT; i++) {
    servo.FeedBack(servoIds[i]);
    startPos[i] = servo.ReadPos(-1);

    // 0〜4095の範囲内になる方向へ移動
    if (startPos[i] + MOVE_STEPS <= 4095) {
      targetPos[i] = startPos[i] + MOVE_STEPS;
    } else {
      targetPos[i] = startPos[i] - MOVE_STEPS;
    }

    Serial.printf(
      "ID %u: start=%d target=%d\n",
      servoIds[i],
      startPos[i],
      targetPos[i]
    );
  }

  lastMove = millis();
}

void loop() {
  if (millis() - lastMove < MOVE_INTERVAL) {
    return;
  }

  int16_t positions[SERVO_COUNT];
  uint16_t speeds[SERVO_COUNT];
  uint8_t accelerations[SERVO_COUNT];

  for (int i = 0; i < SERVO_COUNT; i++) {
    positions[i] = moveToTarget
      ? targetPos[i]
      : startPos[i];

    speeds[i] = MOVE_SPEED;
    accelerations[i] = MOVE_ACCEL;
  }

  // 4台のサーボを同期移動
  servo.SyncWritePosEx(
    servoIds,
    SERVO_COUNT,
    positions,
    speeds,
    accelerations
  );

  moveToTarget = !moveToTarget;
  lastMove = millis();
}

servoIdsには、前章で設定した4台のサーボIDを指定している。

uint8_t servoIds[SERVO_COUNT] = {1, 2, 3, 4};

起動時にはFeedBack()とReadPos()を使用して各サーボの現在位置を取得する。

そのまま+1000するとサーボの位置範囲である0〜4095を超える可能性があるため、現在位置に応じて移動方向を切り替えている。

if (startPos[i] + MOVE_STEPS <= 4095) {
  targetPos[i] = startPos[i] + MOVE_STEPS;
} else {
  targetPos[i] = startPos[i] - MOVE_STEPS;
}

4台のサーボへの移動指令にはSyncWritePosEx()を使用する。

servo.SyncWritePosEx(
  servoIds,
  SERVO_COUNT,
  positions,
  speeds,
  accelerations
);

この関数に各サーボのID、目標位置、速度、加速度をまとめて渡すことで、複数台のサーボへ一括で移動指令を送信できる。

実際に動作させたところ、4台のSTS3215が同じタイミングで開始位置と目標位置を往復することを確認できた。

8. まとめ

ESP32-S3とシリアルバスサーボドライバーボードを使用して、4台のSTS3215を制御できることを確認した。

今回実施した内容は以下の通り。

  • PlatformIOからSCServoライブラリを導入
  • STS3215のサーボIDを1〜4に変更
  • ESP32-S3のUARTから各サーボと通信
  • SyncWritePosEx()を使用して4台のサーボを同期制御

シリアルバスサーボは、サーボごとに個別のPWM信号線を用意する必要がなく、1本のバスに複数台を接続して制御できるため、多関節ロボットなどサーボ数が多い構成では扱いやすい。

0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?