"""Simulate noisy 12-bit joint encoder reads and convert counts to joint angles. A Feetech STS3215 bus servo reports its shaft position as a 12-bit count: 4096 steps per turn, so one step is 360 / 4096 = 0.088 degrees. This module models that read-out with example noise values (they are not measured on a real servo) and shows how averaging several reads reduces the noise. Contents -------- counts_to_degrees convert raw counts to degrees about a calibrated mid count degrees_to_counts the inverse, rounded to whole counts and clipped to 0..4095 simulate_reads noisy, quantised encoder reads for a joint held at one angle average_reads mean angle over a window of reads Run this file to see a worked example: python encoder.py """ from __future__ import annotations import numpy as np from numpy.typing import NDArray ENCODER_BITS: int = 12 COUNTS_PER_REV: int = 2**ENCODER_BITS # 4096 MAX_COUNT: int = COUNTS_PER_REV - 1 # 4095 DEG_PER_COUNT: float = 360.0 / COUNTS_PER_REV # 0.087890625 degrees DEFAULT_MID: int = COUNTS_PER_REV // 2 # 2048, the centre of the count range def counts_to_degrees(counts: NDArray | int, mid: float = DEFAULT_MID) -> NDArray: """Joint angle in degrees for raw encoder counts. `mid` is the count that calibration maps to 0 degrees. Positive angles are counts above `mid`. """ return (np.asarray(counts, dtype=float) - mid) * DEG_PER_COUNT def degrees_to_counts(angle_deg: NDArray | float, mid: float = DEFAULT_MID) -> NDArray: """Nearest whole encoder count for an angle, clipped to the 12-bit range.""" counts = np.rint(np.asarray(angle_deg, dtype=float) / DEG_PER_COUNT + mid) return np.clip(counts, 0, MAX_COUNT).astype(np.int64) def simulate_reads(true_angle_deg: float, n_reads: int, noise_counts: float, rng: np.random.Generator, mid: float = DEFAULT_MID) -> NDArray: """Return `n_reads` noisy integer counts for a joint held at `true_angle_deg`. The model: ideal count + Gaussian noise with standard deviation `noise_counts`, rounded to an integer (quantisation) and clipped to 0..4095. """ ideal = np.asarray(true_angle_deg, dtype=float) / DEG_PER_COUNT + mid noisy = ideal + rng.normal(0.0, noise_counts, size=n_reads) return np.clip(np.rint(noisy), 0, MAX_COUNT).astype(np.int64) def average_reads(counts: NDArray, window: int, mid: float = DEFAULT_MID) -> NDArray: """Mean angle (degrees) over consecutive, non-overlapping windows of reads.""" counts = np.asarray(counts) n_full = (counts.size // window) * window if n_full == 0: raise ValueError("need at least one full window of reads") blocks = counts[:n_full].reshape(-1, window) return counts_to_degrees(blocks.mean(axis=1), mid) def main() -> None: rng = np.random.default_rng(seed=7) true_angle = 30.0 # degrees, example joint angle noise = 2.0 # counts, example noise level (assumed, not measured) reads = simulate_reads(true_angle, n_reads=1000, noise_counts=noise, rng=rng) angles = counts_to_degrees(reads) print(f"resolution: {DEG_PER_COUNT:.4f} deg per count ({COUNTS_PER_REV} counts per turn)") print(f"true angle {true_angle:.2f} deg -> ideal count {true_angle / DEG_PER_COUNT + DEFAULT_MID:.1f}") print(f"first five reads: {reads[:5].tolist()}") print(f"single read: mean {angles.mean():7.3f} deg, std {angles.std(ddof=1):.3f} deg") for window in (4, 16): avg = average_reads(reads, window) print(f"average of {window:2d}: mean {avg.mean():7.3f} deg, std {avg.std(ddof=1):.3f} deg") if __name__ == "__main__": main()