Feetechのサーボをマイコンから直接動かす

![SS 192.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/565043/a41cd967-f549-8677-1efa-2e99bf69518d.png) ネットに情報がすくない気がする

## やること
秋月などでも買えるFeetechのSTS3215サーボをESP32マイコンからジカに動かしてみます。
後半でSCS0009も動かします。

## Feetech STS3215
STS3215は6V~7.4V入力のコマンド式デジタルサーボ（シリアルバスサーボ）です。
360度回転も可能らしく、サイズやパワーにあまりこだわらないタイプの電子工作にはもってこいです。コスパや入手性に大変優れたサーボだと思います。
通信方式は小型サーボによくある信号線１線の半二重方式です。

もう少し小型のSTS3032や、同型で12Vが入るSTS3235の他、いろいろなラインナップがあります。シリーズによってプロトコルに若干の違いがあるようですが、STS3215を使えれば他のFeetechのサーボも使えるようになると思います。



## 作業環境
* Arduino IDE
* ESP32DevkitC
* STS3215

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/565043/967671f7-d13e-c58c-cf14-a7ea9d1cc65a.png" width=400>


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

https://akizukidenshi.com/catalog/g/gM-16312/

↓コマンドのメモリテーブルについてまとめました。

https://qiita.com/Ninagawa123/items/474346a9a2775e87ecdc

↓記事のまとめにあたって参考にさせていただきました。（ありがとうございます）

https://qiita.com/srs/items/5664b3adfea889763a98

 https://kim-xps12.github.io/b-sky-lab/technology/2023/05/31/feetech_serial_board.html


## ライブラリは？
どこかにあるような、ないような。
よくわからないので今回はライブラリなしですすめます。

## STS3215とESP32の接続

サーボを動かすだけであれば半二重回路なしでも大丈夫です。

|サーボ線|機能|ESP32|
|-|:-:|-|
|外側の黒線|GND|GNDピン|
|中央の黒線|Vcc|5Vピン|
|外側の白線|信号線|17番ピン(Serial2)|


<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/565043/b7314f40-37f4-cf65-f462-b9a91c8e54f1.png" width=400>


# サンプルコード

```c++:ESP32_sts3215.ino
// サーボIDと角度を設定
const byte servoID = 1; // サーボのID
int centerPosition = 2048;//サーボ位置のセンター値
int goalPosition = 0;//サーボの目標位置
float radiansval = 0.0; // サインカーブ算出用のラジアン値
float radiansIncrement = 0.06; // ループ毎のラジアン値の増加量
float maxPosition = 1600;// センター値を2048としたとき、±どこまで振るか(0-2048)

void setup() {
  // Serial2の開始（ボーレートを1000000に設定）
  Serial2.begin(1000000);
  delay(100); // サーボの初期化待ち
}

void loop() {
  // サインカーブ用の値を算出
  radiansval += radiansIncrement;  //ラジアン値を増加
  radiansval = (radiansval > 2 * PI) ? 0 : radiansval; //ラジアン値が2πを超えたら0にリセット

  // サーボの値をセットする
  goalPosition = centerPosition + int(sin(radiansval) * maxPosition);
  // サーボにコマンドを送信
  sts_moveToPos(servoID, goalPosition);

  delay(20);
}

void sts_moveToPos(byte id, int position) {
  // コマンドパケットを作成
  byte message[13];
  message[0] = 0xFF;  // ヘッダ
  message[1] = 0xFF;  // ヘッダ
  message[2] = id;    // サーボID
  message[3] = 9;     // パケットデータ長
  message[4] = 3;     // コマンド（3は書き込み命令）
  message[5] = 42;    // レジスタ先頭番号
  message[6] = position & 0xFF; // 位置情報バイト下位
  message[7] = (position >> 8) & 0xFF; // 位置情報バイト上位
  message[8] = 0x00;  // 時間情報バイト下位
  message[9] = 0x00;  // 時間情報バイト上位
  message[10] = 0x00; // 速度情報バイト下位
  message[11] = 0x00; // 速度情報バイト上位

  // チェックサムの計算
  byte checksum = 0;
  for (int i = 2; i < 12; i++) {
    checksum += message[i];
  }
  message[12] = ~checksum; // チェックサム

  // コマンドパケットを送信
  for (int i = 0; i < 13; i++) {
    Serial2.write(message[i]);
  }
}

```

# しくみ
サーボ内のマイコンに対して、どのアドレスにどの値を送るかというコマンドを作ってシリアルで送信しています。
アドレスと値の組み合わせで、サーボにさまざまなコマンドを送ることができます。
今回はサーボの目標位置を示す42番レジスタに、16ビットで示された目標位置を8ビットずつリトルエンディアンでパケットに格納して送信しています。

# 起動と動作結果
1秒間隔ぐらいでサーボが加減速しながらブランコのように左右に回転すれば実験成功です。

# トラブルシューティング
毎度のことですがテスト用にESP32からサーボに給電しているので電力がギリギリです。
安定した動作を確保するには別途、サーボに電源を供給してください。サーボ電源のGNDとESP32のGNDを接続するのをお忘れなく。また逆挿やショートにも十分ご注意ください。


## Feetech SCS0009を動かしたい場合
サーボ内のメモリテーブルが若干違うようです。
こちらのコードで動くと思います。

```c++:ESP32_scs0009.ino
// サーボIDと角度を設定
const byte servoID = 1; // サーボのID
int centerPosition = 512;//サーボ位置のセンター値
int goalPosition = 0;//サーボの目標位置
float radiansval = 0.0; // サインカーブ算出用のラジアン値
float radiansIncrement = 0.06; // ループ毎のラジアン値の増加量
float maxPosition = 400;// センター値を512としたとき、±どこまで振るか(0-512?)

void setup() {
  // Serial2の開始（ボーレートを1000000に設定）
  Serial2.begin(1000000);
  delay(100); // サーボの初期化待ち
}

void loop() {
  // サインカーブ用の値を算出
  radiansval += radiansIncrement;  //ラジアン値を増加
  radiansval = (radiansval > 2 * PI) ? 0 : radiansval; //ラジアン値が2πを超えたら0にリセット

  // サーボの値をセットする
  goalPosition = centerPosition + int(sin(radiansval) * maxPosition);
  // サーボにコマンドを送信
  scs_moveToPos(servoID, goalPosition);

  delay(20);
}

void scs_moveToPos(byte id, int position) {
  // コマンドパケットを作成
  byte message[13];
  message[0] = 0xFF;  // ヘッダ
  message[1] = 0xFF;  // ヘッダ
  message[2] = id;    // サーボID
  message[3] = 9;     // パケットデータ長
  message[4] = 3;     // コマンド（3は書き込み命令）
  message[5] = 42;    // レジスタ先頭番号
  message[6] = (position >> 8) & 0xFF; // 位置情報バイト上位
  message[7] = position & 0xFF; // 位置情報バイト下位
  message[8] = 0x00;  // 時間情報バイト下位
  message[9] = 0x00;  // 時間情報バイト上位
  message[10] = 0x00; // 速度情報バイト下位
  message[11] = 0x00; // 速度情報バイト上位

  // チェックサムの計算
  byte checksum = 0;
  for (int i = 2; i < 12; i++) {
    checksum += message[i];
  }
  message[12] = ~checksum; // チェックサム

  // コマンドパケットを送信
  for (int i = 0; i < 13; i++) {
    Serial2.write(message[i]);
  }
}

```


# さいごに

半二重回路があればデータの受信も可能なようです。
とりあえず今回はコマンドで動かすところまででした。

よき電子工作ライフを〜


# 次回記事：
https://qiita.com/Ninagawa123/items/474346a9a2775e87ecdc

# 次々回記事：
https://qiita.com/Ninagawa123/items/e3281d128c6ca1876564