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
9.4 KB
Raw Blame
"""Keyboard or gamepad teleop for MOSS on a Raspberry Pi, through SerialBridge.

Reuses `moss_dimos.teleop_logic` (pure, no dimOS import): two inputs only,
forward and yaw; deadzone; deadman with a brake window.

Sequence, identical for keyboard and pad:
1. nothing moves until the arm key/button is pressed with the deadman released;
2. while the deadman is held, `drive L R` is sent every loop (default 50 Hz);
3. on release, `drive 0 0` for the brake window (default 0.5 s), then `stop`;
4. `stop` disarms the firmware, so each new drive needs a new arm.
The bridge adds its own deadman (300 ms without drive -> stop), and the
firmware its own (300 ms without drive -> stop and disarm).

Duty mapping: track speeds from `twist_to_tracks` are scaled so that
`max_track_speed_mps` maps to 25 % PWM duty. This is open loop: the firmware
has no speed control, and the speed MOSS actually reaches at 25 % duty is not
measured. The metres per second here are a scale, not a promise.

    python -m moss_pi.teleop --port /dev/ttyUSB0 --keyboard
    python -m moss_pi.teleop --port /dev/ttyUSB0 --gamepad --log drive.jsonl
"""
from __future__ import annotations

import argparse
import math
import os
import sys
import time

from moss_dimos.diffdrive import twist_to_tracks
from moss_dimos.teleop_logic import DeadmanBrake, TeleopConfig, axes_to_twist

from .serial_bridge import MAX_PERCENT, SerialBridge, open_serial

# Standard dual-stick pad, pygame numbering (axes as moss_dimos/gamepad.py; the deadman differs: L1 here, R1 there).
AXIS_LEFT_Y = 1
AXIS_RIGHT_X = 2
BUTTON_ARM = 0       # A / cross
BUTTON_DEADMAN = 4   # L1
BUTTON_BOOST = 5     # R1


def tracks_to_percent(left_mps: float, right_mps: float, max_track_speed_mps: float,
                      max_pct: int = MAX_PERCENT) -> tuple[int, int]:
    """Track speeds in m/s -> integer duty percent for `drive`, max speed -> max_pct.

    Rounds half away from zero and clamps to [-max_pct, max_pct].
    """
    def one(v: float) -> int:
        if not math.isfinite(v):
            raise ValueError(f"track speed must be finite, got {v!r}")
        p = v / max_track_speed_mps * max_pct
        p = math.copysign(math.floor(abs(p) + 0.5), p)
        return int(max(-max_pct, min(max_pct, p)))
    return one(left_mps), one(right_mps)


class TeleopSession:
    """Turns one sample of the inputs into at most one bridge call. Pure apart from the bridge."""

    def __init__(self, bridge: SerialBridge, cfg: TeleopConfig | None = None) -> None:
        self.bridge = bridge
        self.cfg = cfg or TeleopConfig()
        self._brake = DeadmanBrake(self.cfg.brake_s)
        self._arm_prev = False
        self._driving = False     # a drive was sent since the last stop

    def step(self, now_s: float, arm_pressed: bool, deadman_held: bool,
             left_y: float = 0.0, right_x: float = 0.0, boost: bool = False) -> str:
        """Returns what was done: 'arm', 'drive L R', 'brake', 'stop', 'not_armed' or 'idle'."""
        arm_edge = arm_pressed and not self._arm_prev
        self._arm_prev = arm_pressed
        if arm_edge and not deadman_held and not self._driving:
            self.bridge.arm()
            return "arm"

        state = self._brake.tick(deadman_held, now_s)
        if state == "command":
            if not self.bridge.armed:
                return "not_armed"
            vx, wz = axes_to_twist(left_y, right_x, self.cfg, boost_held=boost)
            left, right = tracks_to_percent(*twist_to_tracks(vx, wz, self.cfg.drive),
                                            self.cfg.drive.max_track_speed_mps)
            if self.bridge.drive(left, right):
                self._driving = True
                return f"drive {left} {right}"
            return "not_armed"
        if state == "brake" and self._driving:
            self.bridge.drive(0, 0)
            return "brake"
        if state == "silent" and self._driving:
            self._driving = False
            self.bridge.stop()
            return "stop"
        return "idle"

    def emergency_stop(self) -> None:
        self._driving = False
        self.bridge.stop()


class KeyboardAxes:
    """Terminal keys -> (arm, deadman, left_y, right_x).

    A terminal reports key presses, not releases: a direction key counts as held
    for `hold_s` after its last repeat. `hold_s` must cover the keyboard's
    auto-repeat delay (often 0.5 s), so a keyboard brakes later than a pad.

    w/s forward/back, a/d turn left/right, r arm, space stop now, q quit.
    """

    def __init__(self, speed: float = 0.5, hold_s: float = 0.6) -> None:
        if not 0 < speed <= 1:
            raise ValueError("speed must be in (0, 1]")
        self.speed, self.hold_s = speed, hold_s
        self._seen: dict[str, float] = {}
        self._arm = False
        self.stop_requested = False
        self.quit_requested = False

    def feed(self, key: str, now_s: float) -> None:
        k = key.lower()
        if k in "wasd" and len(k) == 1:
            self._seen[k] = now_s
        elif k == "r":
            self._arm = True
        elif k == " ":
            self._seen.clear()
            self.stop_requested = True
        elif k == "q":
            self._seen.clear()
            self.quit_requested = True

    def sample(self, now_s: float) -> tuple[bool, bool, float, float]:
        live = {k for k, t in self._seen.items() if now_s - t <= self.hold_s}
        arm, self._arm = self._arm, False
        # Stick convention of teleop_logic: forward is left_y < 0, right turn is right_x > 0.
        ly = self.speed * ((("s" in live) - ("w" in live)))
        rx = self.speed * ((("d" in live) - ("a" in live)))
        return arm, bool(live), float(ly), float(rx)


def _run_keyboard(session: TeleopSession, bridge: SerialBridge, speed: float) -> None:
    import select
    import termios
    import tty

    keys = KeyboardAxes(speed=speed)
    fd = sys.stdin.fileno()
    saved = termios.tcgetattr(fd)
    period = 1.0 / session.cfg.rate_hz
    print("r arm | hold w/s/a/d to drive | space stop | q quit", end="\r\n")
    try:
        tty.setcbreak(fd)
        while not keys.quit_requested:
            while select.select([sys.stdin], [], [], 0)[0]:
                keys.feed(os.read(fd, 1).decode(errors="ignore"), time.monotonic())
            if keys.stop_requested:
                keys.stop_requested = False
                session.emergency_stop()
            arm, held, ly, rx = keys.sample(time.monotonic())
            _report(session.step(time.monotonic(), arm, held, ly, rx))
            bridge.tick()
            time.sleep(period)
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, saved)


def _run_gamepad(session: TeleopSession, bridge: SerialBridge) -> None:
    os.environ.setdefault("SDL_VIDEODRIVER", "dummy")
    os.environ.setdefault("SDL_AUDIODRIVER", "dummy")
    os.environ.setdefault("PYGAME_HIDE_SUPPORT_PROMPT", "1")
    import pygame  # optional dependency: pip install 'moss-dimos[pi]'

    pygame.init()
    pygame.joystick.init()
    period = 1.0 / session.cfg.rate_hz
    pad = None
    print("A/cross arm | hold L1 + sticks to drive | R1 boost | Ctrl-C quit")
    while True:
        if pad is None:
            pygame.joystick.quit(); pygame.joystick.init()
            if pygame.joystick.get_count() == 0:
                bridge.tick(); time.sleep(0.5); continue
            pad = pygame.joystick.Joystick(0); pad.init()
            print(f"gamepad connected: {pad.get_name()}")
        pygame.event.pump()
        try:
            arm = bool(pad.get_button(BUTTON_ARM))
            held = bool(pad.get_button(BUTTON_DEADMAN))
            boost = bool(pad.get_button(BUTTON_BOOST))
            ly, rx = pad.get_axis(AXIS_LEFT_Y), pad.get_axis(AXIS_RIGHT_X)
        except pygame.error:
            print("gamepad lost: stop")
            session.emergency_stop()
            pad = None
            continue
        _report(session.step(time.monotonic(), arm, held, ly, rx, boost))
        bridge.tick()
        time.sleep(period)


_last_report = ""


def _report(action: str) -> None:
    global _last_report
    kind = action.split()[0]
    if kind != _last_report:  # one line per change, not one per loop
        print(action, end="\r\n")
    _last_report = kind


def main(argv: list[str] | None = None) -> None:
    ap = argparse.ArgumentParser(description="Drive MOSS from a Pi through the ESP32-S3 firmware.")
    ap.add_argument("--port", required=True, help="serial device, e.g. /dev/ttyUSB0")
    mode = ap.add_mutually_exclusive_group(required=True)
    mode.add_argument("--keyboard", action="store_true")
    mode.add_argument("--gamepad", action="store_true")
    ap.add_argument("--key-speed", type=float, default=0.5, help="keyboard stick deflection, 0..1 (default 0.5)")
    ap.add_argument("--log", help="also append telemetry to this JSONL file")
    args = ap.parse_args(argv)

    bridge = SerialBridge(lambda: open_serial(args.port))
    log = None
    if args.log:
        from .telemetry_log import TelemetryLog
        log = TelemetryLog(args.log)
        bridge.add_listener(log.on_line)
    bridge.add_listener(lambda line, parsed: print(line, end="\r\n") if line.startswith("ERR") else None)
    session = TeleopSession(bridge)
    try:
        if args.keyboard:
            _run_keyboard(session, bridge, args.key_speed)
        else:
            _run_gamepad(session, bridge)
    except KeyboardInterrupt:
        pass
    finally:
        bridge.close()  # sends a last stop
        if log:
            log.close()


if __name__ == "__main__":
    main()