Skip to content

armgit / moss

Public Imported from GitHub
0

MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.

Python
2.6 KB
Raw Blame
"""Differential-drive kinematics for MOSS (tracked base, two motors).

Pure functions, no hardware, no dimOS import: cold-testable anywhere.

Convention: vx forward in m/s, wz counter-clockwise in rad/s, track_width in m
(distance between the two track centrelines). Left/right speeds in m/s at the
track. There is no strafe on tracks: callers never pass a lateral speed.

Every input is validated: a non-finite command or an invalid configuration
raises ValueError instead of producing a motor command.
"""
from __future__ import annotations

import math
from dataclasses import dataclass


def _finite(name: str, value: float) -> float:
    if not isinstance(value, (int, float)) or isinstance(value, bool) or not math.isfinite(value):
        raise ValueError(f"{name} must be a finite number, got {value!r}")
    return float(value)


@dataclass(frozen=True)
class DiffDriveConfig:
    track_width_m: float = 0.30
    max_track_speed_mps: float = 0.6

    def __post_init__(self) -> None:
        w = _finite("track_width_m", self.track_width_m)
        v = _finite("max_track_speed_mps", self.max_track_speed_mps)
        if w <= 0:
            raise ValueError(f"track_width_m must be > 0, got {w}")
        if v <= 0:
            raise ValueError(f"max_track_speed_mps must be > 0, got {v} (a zero limit is a stop, not a config)")


def twist_to_tracks(vx: float, wz: float, cfg: DiffDriveConfig) -> tuple[float, float]:
    """Twist -> (left, right) track speeds in m/s, scaled to stay within the limit.

    Scaling (not clipping) keeps the turn radius when a command is too fast:
    a full-speed forward plus a turn slows both tracks together instead of
    flattening the turn.
    """
    vx = _finite("vx", vx)
    wz = _finite("wz", wz)
    half = 0.5 * cfg.track_width_m
    left = vx - wz * half
    right = vx + wz * half
    peak = max(abs(left), abs(right))
    if peak > cfg.max_track_speed_mps:
        k = cfg.max_track_speed_mps / peak
        left, right = left * k, right * k
    return left, right


def tracks_to_twist(left: float, right: float, cfg: DiffDriveConfig) -> tuple[float, float]:
    """(left, right) track speeds -> (vx, wz). Inverse of twist_to_tracks when unscaled."""
    left = _finite("left", left)
    right = _finite("right", right)
    vx = 0.5 * (left + right)
    wz = (right - left) / cfg.track_width_m
    return vx, wz


def clamp_twist(vx: float, wz: float, cfg: DiffDriveConfig) -> tuple[float, float]:
    """Twist -> the same twist scaled so that neither track exceeds the limit."""
    left, right = twist_to_tracks(vx, wz, cfg)
    return tracks_to_twist(left, right, cfg)