この記事でやること
- サーボモータSTS3215のボーレートとIDをマイコンで全探索するためのサンプルコード
- サーボモータSTS3215のボーレートとIDをマイコンで任意の値に書き換えるサンプルコード
を示します。
STS3215をマイコンで操作する記事は別記事で初歩的なところから書こうと思うので、本記事では詳細を割愛してサンプルコードを中心に記載し、後日全体の整合を見て修正する予定です。
環境
- feetech STS3215 *1
- Windows PC
- Arduino IDE
- Arduino（Raspberry Pi Pico:RP2040）
秋月電子の販売ページからライブラリを導入しておきます。
今回のコードはUSB接続のシリアルポートとGPIO端子のシリアルポートが別に用意されているArduino（互換機）を想定してコードを作っているので、Arduino UNO R3などではそのままでは期待通りの動作をしません。
ボーレートとIDの全探索
ボーレートの範囲
レジスタ［0x06］に格納する値を変更することでボーレート（Baudrate）を変更できます。
| HEX | bps |
|---|---|
| 0x0 | 1000000 |
| 0x1 | 500000 |
| 0x2 | 250000 |
| 0x3 | 128000 |
| 0x4 | 115200 |
| 0x5 | 76800 |
| 0x6 | 57600 |
| 0x7 | 12800 |
IDの範囲
IDは0x00～0xFDの範囲で設定できます。
0xFEは接続されている全てのサーボに対して同時に通信する場合に使われるので、個別のサーボに設定することはできません。
0xFFはヘッダ識別のために使われるためIDとして使われません。
出荷時の初期値は0x01です。
コード
以下のコードを実行すると、ボーレートの高い順にIDを全探索してサーボからping応答があった場合に、PCに対してシリアル通信で見つけたIDを出力します。
SerialUSBはUSB接続してシリアルモニタを開いているPCとの通信を、
SerialPinはGPIOを介してSTS3215サーボとの通信を意味します。
#include "SCServo.h"
SMS_STS sts;
#define SerialUSB Serial
#define SerialPin Serial1
void setup(){
SerialUSB.begin(115200);
SerialPin.begin(115200);
sts.pSerial = &SerialPin;
delay(2000);
SerialUSB.println("Baudrate : ID");
}
void loop(){
int uartbaud[] = {1000000,
500000,
250000,
128000,
115200,
76800,
57600,
38400};
uint8_t id = 0xff;
uint8_t baud_i = 0xff;
for(int j=0; j<8; j++){
SerialPin.begin(uartbaud[j]);
SerialUSB.println("");
SerialUSB.print(uartbaud[j]);
SerialUSB.print(" : ");
for(int i=0; i<254; i++){
SerialUSB.print(i,HEX);
SerialUSB.print(" ");
if(sts.Ping(i) != -1){
baud_i = j;
id = i;
SerialUSB.println("");
SerialUSB.print("ID found : Baudrate : ");
SerialUSB.print(uartbaud[j], DEC);
SerialUSB.print(" ,\tID : 0x");
SerialUSB.println(id, HEX);
}
delay(5);
}
}
if(id == 0xff){
SerialUSB.println("Ping failed.");
}
delay(1000);
}
ボーレートとIDの変更
STS3215サーボはデフォルトでは ［ボーレート:1Mbps,ID:0x01］ です。
コードでは ［ボーレート:128000bps,ID:0xA1］ に変更します。
Pingに成功しなかった場合 ［-1］ が返され、Pingに成功した場合はIDが返されます。
IDは10進数表示のASCII形式でPCのシリアルモニタ側に送られるので、0xA1ではなく161と表示されれば書き換えが成功しています。
EEPROMのロックを解除してからIDを変更しているので、サーボへの電源供給を絶ってもボーレートとIDの変更は維持されるはずです。Pingを4回送るブロックを書き換えの前後に2回入れているのは、このコードのまま書き換え状況を確認できるようにするためです。
#include "SCServo.h"
SMS_STS sts;
#define SerialUSB Serial
#define SerialPin Serial1
void setup(){
SerialUSB.begin(115200);
SerialPin.begin(115200);
sts.pSerial = &SerialPin;
delay(2000);
SerialPin.begin(1000000);
SerialUSB.println(sts.Ping(1));
SerialUSB.println(sts.Ping(0xA1));
SerialPin.begin(128000);
SerialUSB.println(sts.Ping(1));
SerialUSB.println(sts.Ping(0xA1));
SerialPin.begin(1000000);
uint8_t id = 1;
sts.unLockEprom(id);
sts.writeByte(id,SMS_STS_ID,0xA1);
id = 0xA1;
sts.writeByte(id,SMS_STS_BAUD_RATE,3); //3:128000
SerialPin.begin(128000);
sts.LockEprom(id);
SerialPin.begin(1000000);
SerialUSB.println(sts.Ping(1));
SerialUSB.println(sts.Ping(0xA1));
SerialPin.begin(128000);
SerialUSB.println(sts.Ping(1));
SerialUSB.println(sts.Ping(0xA1));
}
void loop(){
// 全探索のコードを入れておくとそのまま全探索を確認できます。
delay(1000);
}