"""SO-101 の制御基板 (Waveshare Bus Servo Adapter・CH343) のサーボを「読むだけ」で確かめる (依頼 248). サーボへの書き込みは一切しない: - connect(handshake=False) でポートを開くだけ - 見つけるのは broadcast ping (読み出し), 各値は read_register (読み出し) - 閉じるときは disconnect(disable_torque=False). 既定の True はトルクを切る書き込みをするので使わない 使い方 (conda の lerobot 環境で): python so101/read_servos_248.py # USB ID が 1a86:55d3 のポートを全部読む python so101/read_servos_248.py --port /dev/tty.usbmodemXXXX """ import argparse import json import serial.tools.list_ports from lerobot.motors import Motor, MotorNormMode, SerialMotorsBus # CH343 (Waveshare Bus Servo Adapter が使う USB シリアル) ADAPTER_VID_PID = (0x1A86, 0x55D3) REGISTERS = [ "Model_Number", "Firmware_Major_Version", "Firmware_Minor_Version", "Present_Position", "Present_Voltage", "Present_Temperature", "Torque_Enable", "Lock", ] def adapter_ports(): found = [] for p in serial.tools.list_ports.comports(): if (p.vid, p.pid) == ADAPTER_VID_PID and p.device.startswith("/dev/cu."): found.append(p.device.replace("/dev/cu.", "/dev/tty.")) return sorted(found) def open_bus(port, ids): bus = SerialMotorsBus(port, {f"m{i}": Motor(i, "sts3215", MotorNormMode.RANGE_M100_100) for i in ids}) bus.connect(handshake=False) return bus def read_port(port): # 1回目: ID 1 の sts3215 を1つだけ置いた bus で, 定義を使って broadcast ping で全 ID を探す bus = open_bus(port, [1]) try: ids = bus._scan(bus._definitions[1]) finally: bus.disconnect(disable_torque=False) if not ids: return [] # 2回目: 見つかった ID を全部 bus に置き直して, 各値を読む bus = open_bus(port, sorted(ids)) try: rows = [] for motor_id, model_nb in sorted(ids.items()): row = {"id": motor_id, "model_number_scan": model_nb} for reg in REGISTERS: try: row[reg] = bus._connected_bus.read_register_with_error(motor_id, reg.lower(), retries=2)[0] except Exception as e: # 読めない値は理由を残して先へ進む row[reg] = f"ERR {e}" rows.append(row) return rows finally: bus.disconnect(disable_torque=False) def main(): ap = argparse.ArgumentParser() ap.add_argument("--port", action="append", help="読むポート (省略時は CH343 のポートを全部)") ap.add_argument("--json", help="結果を JSON で書く先") args = ap.parse_args() ports = args.port or adapter_ports() if not ports: print("制御基板 (1a86:55d3) のポートが見えない") return result = {} for port in ports: print(f"\n== {port}") try: rows = read_port(port) except Exception as e: print(f" 読めない: {e}") result[port] = {"error": str(e)} continue result[port] = rows if not rows: print(" サーボが1つも答えない (電源が入っていない?)") continue volts = [r["Present_Voltage"] / 10 for r in rows if isinstance(r["Present_Voltage"], int)] print(f" サーボ {len(rows)} 個, 電圧の平均 {sum(volts) / len(volts):.1f} V" if volts else " 電圧が読めない") print(" ID 型番 FW 位置 電圧V 温度C トルク Lock") for r in rows: v = r["Present_Voltage"] v = f"{v / 10:.1f}" if isinstance(v, int) else v print( f" {r['id']:>2} {r['Model_Number']} {r['Firmware_Major_Version']}.{r['Firmware_Minor_Version']}" f" {r['Present_Position']:>5} {v:>5} {r['Present_Temperature']:>5}" f" {r['Torque_Enable']:>6} {r['Lock']:>4}" ) if args.json: with open(args.json, "w") as f: json.dump(result, f, ensure_ascii=False, indent=2) if __name__ == "__main__": main()