"""Frame sizes and wire time for Sync Write and Sync Read on a Feetech STS bus. Packet layout as implemented by rustypot (protocol v1), the serial driver that LeRobot uses for Feetech servos: 0xFF 0xFF | ID | LENGTH | INSTRUCTION | PARAMETERS... | CHECKSUM LENGTH = number of parameters + 2, CHECKSUM = ~(ID + LENGTH + INSTRUCTION + params) & 0xFF. Sync Read is instruction 0x82 and Sync Write 0x83, both sent to the broadcast ID 0xFE. After a Sync Read every addressed servo answers with its own status packet: 0xFF 0xFF | ID | LENGTH | ERROR | DATA... | CHECKSUM """ from __future__ import annotations from collections.abc import Sequence BROADCAST_ID = 0xFE SYNC_READ = 0x82 SYNC_WRITE = 0x83 def checksum(body: Sequence[int]) -> int: return (~sum(body)) & 0xFF def packet(dev_id: int, instruction: int, params: Sequence[int]) -> bytes: body = [dev_id, len(params) + 2, instruction, *params] return bytes([0xFF, 0xFF, *body, checksum(body)]) def sync_write_packet(ids: Sequence[int], addr: int, data: Sequence[Sequence[int]]) -> bytes: """One packet that writes len(data[i]) bytes at addr in every servo ids[i].""" n = len(data[0]) if any(len(d) != n for d in data) or len(ids) != len(data): raise ValueError("every servo needs the same number of data bytes") params = [addr, n] for i, d in zip(ids, data): params += [i, *d] return packet(BROADCAST_ID, SYNC_WRITE, params) def sync_read_packet(ids: Sequence[int], addr: int, length: int) -> bytes: return packet(BROADCAST_ID, SYNC_READ, [addr, length, *ids]) def status_packet_size(data_len: int) -> int: return 6 + data_len def wire_time_us(n_bytes: int, baud: int = 1_000_000, bits_per_byte: int = 10) -> float: """UART time on the wire; 10 bits per byte for 8N1 framing (start + 8 data + stop).""" return n_bytes * bits_per_byte / baud * 1e6 def sync_cycle(n_servos: int, data_len: int = 2, baud: int = 1_000_000, return_delay_us: float = 0.0) -> dict[str, float]: """Bytes and microseconds for one Sync Write plus one Sync Read of n_servos. return_delay_us is added once per status packet. Turnaround gaps, USB latency and operating-system scheduling are not included. """ ids = list(range(1, n_servos + 1)) w = len(sync_write_packet(ids, 42, [[0] * data_len] * n_servos)) r_req = len(sync_read_packet(ids, 56, data_len)) r_rep = n_servos * status_packet_size(data_len) return { "write_bytes": w, "read_request_bytes": r_req, "read_reply_bytes": r_rep, "write_us": wire_time_us(w, baud), "read_us": wire_time_us(r_req + r_rep, baud) + n_servos * return_delay_us, "total_us": wire_time_us(w + r_req + r_rep, baud) + n_servos * return_delay_us, } if __name__ == "__main__": # Goal_Position is register 42 and Present_Position register 56 on the STS3215 (2 bytes each). c = sync_cycle(6) print(f"Sync Write, 6 servos x 2 bytes: {c['write_bytes']} bytes, {c['write_us']:.0f} us") print(f"Sync Read request: {c['read_request_bytes']} bytes; replies: {c['read_reply_bytes']} bytes; " f"{c['read_us']:.0f} us") print(f"total at 1 Mbit/s: {c['total_us']:.0f} us") slow = sync_cycle(6, return_delay_us=500.0) print(f"with a 500 us return delay per reply: {slow['total_us']:.0f} us")