🤖
Feetech製STS3215シリアルサーボをPythonで動かしてみる
関連リンク
LeRobotフレームワークを使う前に，
STS3215 シリアルサーボをfeetech-servo-sdkを使って操作してみる．
LeRobotでも内部ではこのSDKを使って制御しているので，内部理解にもなるはず．
必要なもの
- 5-7.4V, 4A程度のアダプタ (最終的に6個ぐらい接続するので4A以上あるといい)
- Bus Servo アダプタ （シリアル通信と電源を束ねてサーボに送信）
- STS3215
- USBケーブル
接続のブロック図は以下．サーボ間は付属のケーブルで数珠つなぎする．
feetech-servo-sdk の導入
Ubuntu24.04 LTSで実施．venv環境内でセットアップしてます．
apt install python3.12-venv -y
python3 -m venv venv
source venv/bin/activate
pip install feetech-servo-sdk
USBのポート確認
シリアルサーボなので，モーターとのやり取りはシリアル通信で行うことができる．
まずモーターと通信するためには，デバイスパスとボーレートを知っておく必要がある．
Bus Servo アダプタをPCに接続した直後に以下のコマンドで判別できる．
sudo dmesg | tail -n 20
:
[ 6341.571571] usb 1-8.2.4: New USB device found, idVendor=1a86, idProduct=55d3, bcdDevice= 4.45
[ 6341.571579] usb 1-8.2.4: New USB device strings: Mfr=0, Product=2, SerialNumber=3
[ 6341.571582] usb 1-8.2.4: Product: USB Single Serial
[ 6341.571586] usb 1-8.2.4: SerialNumber: 5B14111029
[ 6341.594163] cdc_acm 1-8.2.4:1.0: ttyACM0: USB ACM device
上記の場合，ttyACM0なので，デバイスパスは/dev/ttyACM0にだとわかる．
ボーレートはデフォルト 1000000 bpsになる．とりあえず変更することはなさそう．
モーターIDの取得と設定
モーターIDに関しては数珠つなぎされた中から特定のモーターに指示するために必ず必要になる．工場出荷状態では，どのモーターもID=1がEEPROMに保存されている．複数台同時に使う場合はこのIDが被らないように設定する必要がある．
まずはIDの取得だが，スマートには取得できない．以下のようにIDを1から順にping()関数を使って確認する．正しい反応が帰ってきたときのIDを返す関数を作成．
import sys
import scservo_sdk as scs
def get_motor_id(port="/dev/ttyACM0", baudrate=1000000):
port_handler = scs.PortHandler(port)
# STS / SMS series = 0, SCS series = 1
packet_handler = scs.PacketHandler(0)
port_handler.openPort()
port_handler.setBaudRate(baudrate)
# ping check
motor_id = -1
model_number = -1
# 1 - 254
for test_id in range(1, 254):
found_model_number, comm, error = packet_handler.ping(port_handler, test_id)
if comm == 0:
motor_id = test_id
model_number = found_model_number
break
port_handler.closePort()
FEETECH_MODEL_NUMBER_TABLE = {
"sts3215": 777,
"sts3250": 2825
}
FEETECH_MODEL_PROTOCOL = {
"sts3215": 0,
"sts3250": 0
}
model_name = ""
for k in FEETECH_MODEL_NUMBER_TABLE.keys():
if FEETECH_MODEL_NUMBER_TABLE[k] == model_number:
model_name = k
break
model_protocol = 0 # sts3215, sts3250
return (motor_id, model_name, model_number, model_protocol)
(motor_id, model_name, model_number, model_protocol) = get_motor_id()
print("model_name=%s, new motor id=%d"%(model_name, motor_id))
接続したモーターのIDは3ということになる．
model_name=sts3215, new motor id=3
次にIDの設定だが，1−254までのIDを設定することができる．注意点としては，設定するときに既存のIDを指定する必要がある．なので，前述のID取得関数を使って現在のIDを取得して，それから新しいIDを設定する．
import sys
import scservo_sdk as scs
def set_motor_id(current_id, new_id, port="/dev/ttyACM0", baudrate=1000000):
# init handler
portHandler = scs.PortHandler(port)
# STS / SMS series = 0, SCS series = 1
packetHandler = scs.PacketHandler(0)
# open port
if not portHandler.openPort():
print("failed to open port.")
exit()
# set baundrate
if not portHandler.setBaudRate(baudrate):
print("failed to set baudrate")
exit()
# EEPROM ID address
ADDR_ID = 5
# write1ByteTxRx(port, motor_id, address, data)
scs_comm_result, scs_error = packetHandler.write1ByteTxRx(portHandler, current_id, ADDR_ID, new_id)
if scs_comm_result != scs.COMM_SUCCESS:
print(f"comm error: {packetHandler.getTxRxResult(scs_comm_result)}")
elif scs_error != 0:
print(f"status error: {packetHandler.getRxPacketError(scs_error)}")
else:
print(f"succeeded to set motor id. %d => %d"%(current_id, new_id))
# close port
portHandler.closePort()
return
(motor_id, model_name, model_number, model_protocol) = get_motor_id()
print("model_name=%s, new motor id=%d"%(model_name, motor_id))
set_motor_id(current_id=motor_id, new_id=1)
ID=1を設定
model_name=sts3215, new motor id=3
succeeded to set motor id. 3 => 1
各種情報取得
ついでにID以外のEEPROMの設定情報を取得する関数．
import sys
import scservo_sdk as scs
# Feetech特有の符号付き絶対値表現をPythonの整数に変換する
def decode_sign_magnitude(value, bit_size=16):
sign_bit = 1 << (bit_size - 1)
if value & sign_bit:
return -(value & ~sign_bit)
return value
# モーター情報取得
def get_motor_info(motor_id, model_name, port="/dev/ttyACM0", baudrate=1000000):
# ── EEPROM領域（電源OFF後も保持）──────────────────────────────
ADDR_BAUDRATE = 6 # 通信速度インデックス
ADDR_RETURN_DELAY = 7 # 応答遅延時間 (x2 us, 0=最速)
ADDR_MIN_LIMIT = 9 # 最小位置リミット (2byte)
ADDR_MAX_LIMIT = 11 # 最大位置リミット (2byte)
ADDR_MAX_TEMP_LIMIT = 13 # 過熱保護しきい値 (°C)
ADDR_MAX_VOLTAGE = 14 # 最大電圧しきい値 (x10 V)
ADDR_MIN_VOLTAGE = 15 # 最小電圧しきい値 (x10 V)
ADDR_MAX_TORQUE = 16 # トルク上限 (2byte, 0~1000)
ADDR_P_GAIN = 21 # PIDのP係数
ADDR_D_GAIN = 22 # PIDのD係数
ADDR_I_GAIN = 23 # PIDのI係数
ADDR_CW_DEAD_BAND = 26 # CW方向の不感帯幅 (小さいと振動しやすい)
ADDR_CCW_DEAD_BAND = 27 # CCW方向の不感帯幅
ADDR_HOMING_OFFSET = 31 # ゼロ点オフセット (2byte, 符号付き)
ADDR_OPERATING_MODE = 33 # 動作モード (0=Servo, 1=Wheel)
# ── RAM領域（電源OFF時リセット）────────────────────────────────
ADDR_TORQUE_ENABLE = 40 # トルクON/OFF (0=OFF, 1=ON)
ADDR_ACCELERATION = 41 # 加速度制限 (0=最大加速, 大きいほど緩やか)
ADDR_GOAL_POSITION = 42 # 目標位置 (2byte, 0~4095)
ADDR_GOAL_TIME = 44 # 移動時間指定 (2byte, ms単位, 0=Speed制御)
ADDR_GOAL_SPEED = 46 # 目標速度 (2byte, 0=最大速度)
ADDR_LOCK = 55 # EEPROMロック (0=書込可, 1=ロック)
ADDR_PRESENT_POSITION = 56 # 現在位置 (2byte, 0~4095)
ADDR_PRESENT_SPEED = 58 # 現在速度 (2byte, 符号付き)
ADDR_PRESENT_LOAD = 60 # 現在負荷 (2byte, 符号付き)
ADDR_PRESENT_VOLTAGE = 62 # 現在電圧 (x10 V)
ADDR_PRESENT_TEMP = 63 # 現在温度 (°C)
ADDR_MOVING = 66 # 動作中フラグ (0=停止, 1=動作中)
ADDR_PRESENT_CURRENT = 69 # 現在電流 (2byte, mA)
# ── Baudrateインデックス変換マップ ────────────────────────────
BAUDRATE_MAP = {
0: 1_000_000,
1: 500_000,
2: 250_000,
3: 128_000,
4: 115_200,
5: 38_400,
6: 19_200,
7: 9_600,
}
# ── ポート初期化 ──────────────────────────────────────────────
portHandler = scs.PortHandler(port)
packetHandler = scs.PacketHandler(0) # 0=STS/SMS系, 1=SCS系
if not portHandler.openPort():
print(f"エラー: ポート {port} を開けませんでした。")
sys.exit(1)
if not portHandler.setBaudRate(baudrate):
print(f"エラー: ボーレート {baudrate} を設定できませんでした。")
portHandler.closePort()
sys.exit(1)
print(f"\n--- Motor ID: {motor_id} から情報を取得中 ---")
# EEPROMアンロック（読み取りのみでも念のため）
packetHandler.write1ByteTxRx(portHandler, motor_id, ADDR_LOCK, 0)
def read1(addr):
val, res, _ = packetHandler.read1ByteTxRx(portHandler, motor_id, addr)
if res != scs.COMM_SUCCESS:
print(f" [WARN] addr={addr} 読み取り失敗: {packetHandler.getTxRxResult(res)}")
return None
return val
def read2(addr):
val, res, _ = packetHandler.read2ByteTxRx(portHandler, motor_id, addr)
if res != scs.COMM_SUCCESS:
print(f" [WARN] addr={addr} 読み取り失敗: {packetHandler.getTxRxResult(res)}")
return None
return val
# ── 読み取り ─────────────────────────────────────────────────
baud_idx = read1(ADDR_BAUDRATE)
return_delay = read1(ADDR_RETURN_DELAY)
min_limit = read2(ADDR_MIN_LIMIT)
max_limit = read2(ADDR_MAX_LIMIT)
max_temp_lim = read1(ADDR_MAX_TEMP_LIMIT)
max_voltage = read1(ADDR_MAX_VOLTAGE)
min_voltage = read1(ADDR_MIN_VOLTAGE)
max_torque = read2(ADDR_MAX_TORQUE)
p_gain = read1(ADDR_P_GAIN)
d_gain = read1(ADDR_D_GAIN)
i_gain = read1(ADDR_I_GAIN)
cw_dead = read1(ADDR_CW_DEAD_BAND)
ccw_dead = read1(ADDR_CCW_DEAD_BAND)
homing_raw = read2(ADDR_HOMING_OFFSET)
op_mode = read1(ADDR_OPERATING_MODE)
torque_en = read1(ADDR_TORQUE_ENABLE)
acceleration = read1(ADDR_ACCELERATION)
goal_pos = read2(ADDR_GOAL_POSITION)
goal_time = read2(ADDR_GOAL_TIME)
goal_speed_raw= read2(ADDR_GOAL_SPEED)
cur_pos = read2(ADDR_PRESENT_POSITION)
cur_speed_raw = read2(ADDR_PRESENT_SPEED)
cur_load_raw = read2(ADDR_PRESENT_LOAD)
cur_voltage = read1(ADDR_PRESENT_VOLTAGE)
cur_temp = read1(ADDR_PRESENT_TEMP)
moving = read1(ADDR_MOVING)
cur_current = read2(ADDR_PRESENT_CURRENT)
portHandler.closePort()
# ── デコード ─────────────────────────────────────────────────
baudrate_bps = BAUDRATE_MAP.get(baud_idx, f"unknown ({baud_idx})")
homing_offset = decode_sign_magnitude(homing_raw, bit_size=16) if homing_raw is not None else None
# Wheel Modeのみ速度が符号付き
goal_speed = decode_sign_magnitude(goal_speed_raw, bit_size=16) if op_mode == 1 else goal_speed_raw
cur_speed = decode_sign_magnitude(cur_speed_raw, bit_size=16) if cur_speed_raw is not None else None
cur_load = decode_sign_magnitude(cur_load_raw, bit_size=16) if cur_load_raw is not None else None
op_mode_str = {0: "Servo Mode", 1: "Wheel Mode"}.get(op_mode, f"Unknown({op_mode})")
# ── 表示 ─────────────────────────────────────────────────────
print(f"Motor Information:")
print(f" ── 基本設定 (EEPROM) ──────────────────────────")
print(f" [Motor_ID] : {motor_id}")
print(f" [Model_Name] : {model_name}")
print(f" [Model_Resolution] : 4096")
print(f" [Baudrate] : {baudrate_bps} bps (idx={baud_idx})")
print(f" [Return_Delay_Time] : {return_delay} (x2 us)")
print(f" [Operating_Mode] : {op_mode} ({op_mode_str})")
print(f" [Min_Position_Limit] : {min_limit}")
print(f" [Max_Position_Limit] : {max_limit}")
print(f" [Homing_Offset] : {homing_offset}")
print(f" ── 保護設定 (EEPROM) ──────────────────────────")
print(f" [Max_Temperature_Limit] : {max_temp_lim} °C")
print(f" [Max_Voltage] : {max_voltage / 10:.1f} V" if max_voltage is not None else " [Max_Voltage] : N/A")
print(f" [Min_Voltage] : {min_voltage / 10:.1f} V" if min_voltage is not None else " [Min_Voltage] : N/A")
print(f" [Max_Torque] : {max_torque}")
print(f" ── PID・制御パラメータ (EEPROM) ────────────────")
print(f" [P_Gain] : {p_gain}")
print(f" [D_Gain] : {d_gain}")
print(f" [I_Gain] : {i_gain}")
print(f" [CW_Dead_Band] : {cw_dead}")
print(f" [CCW_Dead_Band] : {ccw_dead}")
print(f" ── 動作設定 (RAM) ─────────────────────────────")
print(f" [Torque_Enable] : {torque_en} ({'ON' if torque_en else 'OFF'})")
print(f" [Acceleration] : {acceleration} (0=最大加速)")
print(f" [Goal_Time] : {goal_time} ms (0=Speed制御)")
print(f" [Goal_Speed] : {goal_speed} steps/sec (0=最大速度)")
print(f" ── 現在状態 (RAM) ─────────────────────────────")
print(f" [Goal_Position] : {goal_pos}")
print(f" [Present_Position] : {cur_pos}")
print(f" [Present_Speed] : {cur_speed} steps/sec")
print(f" [Present_Load] : {cur_load}")
print(f" [Present_Voltage] : {cur_voltage / 10:.1f} V" if cur_voltage is not None else " [Present_Voltage] : N/A")
print(f" [Present_Temperature] : {cur_temp} °C")
print(f" [Moving] : {moving} ({'動作中' if moving else '停止中'})")
print(f" [Present_Current] : {cur_current} mA")
(motor_id, model_name, model_number, model_protocol) = get_motor_id()
get_motor_info(motor_id, model_name)
--- Motor ID: 1 から情報を取得中 ---
Motor Information:
── 基本設定 (EEPROM) ──────────────────────────
[Motor_ID] : 1
[Model_Name] : sts3215
[Model_Resolution] : 4096
[Baudrate] : 1000000 bps (idx=0)
[Return_Delay_Time] : 0 (x2 us)
[Operating_Mode] : 0 (Servo Mode)
[Min_Position_Limit] : 0
[Max_Position_Limit] : 4095
[Homing_Offset] : 2047
── 保護設定 (EEPROM) ──────────────────────────
[Max_Temperature_Limit] : 70 °C
[Max_Voltage] : 8.0 V
[Min_Voltage] : 4.0 V
[Max_Torque] : 1000
── PID・制御パラメータ (EEPROM) ────────────────
[P_Gain] : 32
[D_Gain] : 32
[I_Gain] : 0
[CW_Dead_Band] : 1
[CCW_Dead_Band] : 1
── 動作設定 (RAM) ─────────────────────────────
[Torque_Enable] : 0 (OFF)
[Acceleration] : 0 (0=最大加速)
[Goal_Time] : 0 ms (0=Speed制御)
[Goal_Speed] : 0 steps/sec (0=最大速度)
── 現在状態 (RAM) ─────────────────────────────
[Goal_Position] : 0
[Present_Position] : 1560
[Present_Speed] : 0 steps/sec
[Present_Load] : 0
[Present_Voltage] : 5.2 V
[Present_Temperature] : 31 °C
[Moving] : 0 (停止中)
[Present_Current] : 0 mA
次回は実際に回転させてみる．
Discussion