fix gamepad layout and refuse a pad whose axes disagree

An Xbox One S pad over Bluetooth numbers its controls differently from the
indices this node shipped with: the right stick is on axes 2 and 3, the
triggers on 4 and 5, and the buttons carry gaps at 2 and 5. Against the old
layout an untouched pad reported a resting trigger as full right-stick
deflection, which asked for 30 deg/s of pitch while lying on the desk, held
the lease for ever so GAMEPAD_IDLE_RELEASE_S never fired, and moved a real
arm into a controller error. X toggled freedrive and LB was the E-STOP, so
the resulting latch could not be released from the pad at all.

Layout now carries the swept indices. Each pad is also measured the first
time it is seen at rest: a stick must rest near centre and a trigger near
the bottom of its travel, and a pad that does not fit reads as absent and is
named in the log rather than driving an arm from a numbering nobody checked.
The measured rest becomes each axis's zero, so a worn stick's tilt is
subtracted rather than deadzoned away and a trigger that reads 0 until it
first moves stops reading as half pressed.

Rest is never re-measured while the pad is in use, because doing that under
a held stick would record the deflection as centre and the arm would move
when the operator let go. The probe drives from the same calibration, so
what it prints is still what the arm would get, and it shows the mismatch.

Tests name their controls through axes_for and buttons_for instead of
numbering them; the positional lists are why a wrong index passed a green
suite.

dora_teleop/joy.py

@@ -12,11 +12,19 @@ unplugged pad. So silence past `JOY_TTL_S` reads as *absent*, which is the same
 answer, because a reading nobody is refreshing is not evidence of anything.
 `dora-joy` publishes on every tick whether or not the pad moved, so silence
 means one thing only.
+
+The other thing this seam does is :class:`Calibration`: no two pads rest in
+quite the same place, and one that rests somewhere the layout does not expect
+is a pad this node must not drive an arm from. Every reading the loop sees has
+been through it, so the rest of the node only ever handles a pad that behaves.
 """
 
 import time
 from dataclasses import dataclass, field
 
+from dora_teleop import mapping
+from dora_teleop.log import log
+
 
 @dataclass(frozen=True)
 class Sample:
@@ -52,17 +60,81 @@ def decode(event: dict) -> Sample:
     )
 
 
+class Calibration:
+    """Where the pad in front of us rests, and whether the layout fits it.
+
+    Captured from the first reading of a pad that is sitting still, and kept
+    until that pad goes away. It is not re-taken while the pad is in use, which
+    would be the one way to get this badly wrong: an operator holding a stick
+    when it happened would have that deflection recorded as centre, and the arm
+    would then move whenever they let go.
+
+    A pad whose resting axes do not fit the layout is reported **absent** rather
+    than corrected. The numbering cannot be guessed from one reading — a trigger
+    resting at -1.0 and a stick held hard over look identical — and guessing
+    wrong drives a real arm. `dora-teleop-probe` shows what the pad reports, and
+    `Layout` in :mod:`dora_teleop.mapping` is where a pad that disagrees is
+    written down.
+    """
+
+    def __init__(self, layout: mapping.Layout = mapping.LAYOUT):
+        self.layout = layout
+        self.mismatch: str | None = None
+        self._zeros: list[float] | None = None
+
+    @property
+    def known(self) -> bool:
+        """Whether a pad has been measured at rest and may be driven from."""
+        return self._zeros is not None
+
+    def forget(self) -> None:
+        """Drop what was learned, so the next pad to appear is measured afresh."""
+        self._zeros = None
+        self.mismatch = None
+
+    def apply(self, sample: Sample) -> Sample:
+        """Return the reading as the layout's ideal pad would have sent it.
+
+        An absent pad passes straight through and takes the calibration with
+        it. A pad that does not fit the layout comes back absent, which every
+        path in this node already treats as a reason to stop.
+        """
+        if not sample.present:
+            self.forget()
+            return sample
+        if self._zeros is None or len(self._zeros) != len(sample.axes):
+            try:
+                self._zeros = mapping.rest_zeros(sample.axes, self.layout)
+            except ValueError as wrong:
+                # Logged once per pad: this runs on every reading, and a pad
+                # that does not fit the layout does not fit it fifty times a
+                # second.
+                if self.mismatch is None:
+                    log(
+                        f"ignoring the gamepad: {wrong}. Run dora-teleop-probe and check Layout in mapping.py"
+                    )
+                self.mismatch = str(wrong)
+                return ABSENT
+            self.mismatch = None
+        return Sample(
+            axes=mapping.canonical(sample.axes, self._zeros, self.layout),
+            buttons=sample.buttons,
+            present=True,
+        )
+
+
 class JoyInput:
     """The latest reading, and how long ago it arrived."""
 
-    def __init__(self, ttl_s: float):
+    def __init__(self, ttl_s: float, layout: mapping.Layout = mapping.LAYOUT):
         self.ttl_s = ttl_s
+        self.calibration = Calibration(layout)
         self._sample = ABSENT
         self._seen_at = 0.0
 
     def accept(self, sample: Sample, moment: float) -> None:
-        """Take one reading off the wire."""
+        """Take one reading off the wire, calibrated to the pad that sent it."""
-        self._sample = sample
+        self._sample = self.calibration.apply(sample)
         self._seen_at = moment
 
     def read(self, moment: float) -> Sample:

dora_teleop/mapping.py

@@ -34,26 +34,34 @@ MM_PER_M = 1000.0
 class Layout:
     """Which axis and button index does what, in SDL's numbering.
 
-    The defaults are the layout SDL reports for an Xbox-style pad, which is also
+    These are what an Xbox One S pad reports over Bluetooth, swept one control
-    what it reports for most PlayStation-style ones. A pad that disagrees is a
+    at a time: sticks on 0-3, both triggers last, and buttons with gaps at 2 and
-    change here rather than a new environment variable: an index nobody can
+    5. A pad that disagrees is a change here rather than a new environment
-    verify from the outside is worse than a line of code somebody can read.
+    variable: an index nobody can verify from the outside is worse than a line
+    of code somebody can read. `dora-teleop-probe` is how you check.
+
+    **The axes can be checked at runtime and the buttons cannot.**
+    :func:`rest_zeros` refuses a pad whose resting axes say this numbering is
+    wrong, because an axis at rest has a value to compare. An unpressed button
+    reads the same on every pad, so a wrong button index is silent — it was
+    found here by `clear` toggling freedrive and `yaw_left` firing the E-STOP.
+    The probe, and pressing each button once, is the only check there is.
     """
 
     left_x: int = 0
     left_y: int = 1
-    left_trigger: int = 2
+    right_x: int = 2
-    right_x: int = 3
+    right_y: int = 3
-    right_y: int = 4
+    left_trigger: int = 4
     right_trigger: int = 5
 
-    yaw_left: int = 4  # LB
-    yaw_right: int = 5  # RB
     grasp: int = 0  # A
     release: int = 1  # B
-    clear: int = 2  # X
+    clear: int = 3  # X
-    freedrive: int = 3  # Y
+    freedrive: int = 4  # Y
-    estop: int = 6  # Back
+    yaw_left: int = 6  # LB
+    yaw_right: int = 7  # RB
+    estop: int = 11  # Back
 
 
 LAYOUT = Layout()
@@ -69,6 +77,16 @@ TOOL_FRAME = "tool"
 # would mean a stick could claim the arm and then not move it.
 ACTIVITY = 0.3
 
+# Where an untouched control sits once :func:`canonical` has had it: a stick at
+# centre, a trigger at the bottom of its travel.
+STICK_REST = 0.0
+TRIGGER_REST = -1.0
+
+# How far an axis may rest from those and still be believed. Wide enough for a
+# worn stick's tilt and for a pad that reports a trigger it has never felt as 0,
+# and far short of the full deflection a wrongly numbered axis shows.
+REST_TOLERANCE = 0.25
+
 
 def deadzone(value: float, width: float) -> float:
     """Ignore travel near centre, and rescale what is left to the full range.
@@ -98,11 +116,13 @@ def expo(value: float, factor: float) -> float:
 def trigger(value: float) -> float:
     """Return a trigger's travel as 0 at rest and 1 fully pressed.
 
-    SDL reports a trigger from -1 to 1, unlike a stick, and a trigger that has
+    SDL reports a trigger from -1 to 1, unlike a stick, so `TRIGGER_REST` is
-    never been touched sometimes reads 0 instead of -1 until it first moves.
+    where an untouched one sits. A trigger that has never moved sometimes reads
-    Clamping keeps that from reading as half pressed.
+    0 instead, which this function alone cannot tell from half pressed —
+    :func:`canonical` is what fixes that, by measuring the pad's own rest before
+    anything reaches here.
     """
-    return max(0.0, min(1.0, (value + 1.0) / 2.0))
+    return max(0.0, min(1.0, (value - TRIGGER_REST) / (1.0 - TRIGGER_REST)))
 
 
 def axis(values: list[float], index: int) -> float:
@@ -115,6 +135,87 @@ def pressed(buttons: list[bool], index: int) -> bool:
     return bool(buttons[index]) if index < len(buttons) else False
 
 
+def axis_names(layout: Layout = LAYOUT) -> dict[int, str]:
+    """Name each axis index the layout uses."""
+    return {
+        layout.left_x: "left stick X",
+        layout.left_y: "left stick Y",
+        layout.left_trigger: "left trigger",
+        layout.right_x: "right stick X",
+        layout.right_y: "right stick Y",
+        layout.right_trigger: "right trigger",
+    }
+
+
+def rest_zeros(axes: list[float], layout: Layout = LAYOUT) -> list[float]:
+    """Return where each axis of an untouched pad sits, to measure deflection from.
+
+    An untouched pad is the one moment this node can check `layout` against the
+    thing in front of it: a stick rests near centre and a trigger near the
+    bottom of its travel, so an axis resting anywhere else means the numbering
+    is wrong, or a control is being held, or the pad is broken. All three look
+    the same from here and all three raise, because all three would otherwise
+    drive the arm from a reading nobody made — a trigger read as a stick asks
+    for full speed while sitting untouched on the desk.
+
+    Parameters
+    ----------
+    axes
+        One reading, taken while nobody is touching the pad.
+    layout
+        Which index is which control.
+
+    Returns
+    -------
+    list of float
+        The resting value of each axis, in the order they were reported.
+
+    Raises
+    ------
+    ValueError
+        If any axis rests where the layout says it cannot, naming the index,
+        what the layout calls it, and what it read.
+    """
+    names = axis_names(layout)
+    triggers = (layout.left_trigger, layout.right_trigger)
+    for index, value in enumerate(axes):
+        if index in triggers:
+            # Either end is a resting trigger: -1 is what SDL reports, and 0 is
+            # what some pads report until the trigger first moves.
+            if abs(value - TRIGGER_REST) <= REST_TOLERANCE or abs(value - STICK_REST) <= REST_TOLERANCE:
+                continue
+        elif abs(value - STICK_REST) <= REST_TOLERANCE:
+            continue
+        name = names.get(index, "an axis the layout does not use")
+        raise ValueError(f"axis {index} rests at {value:+.3f}, which is not where {name} sits")
+    return list(axes)
+
+
+def canonical(axes: list[float], zeros: list[float], layout: Layout = LAYOUT) -> list[float]:
+    """Return this pad's axes as the layout's ideal pad would have reported them.
+
+    Sticks come back resting at `STICK_REST` and triggers at `TRIGGER_REST`,
+    whatever this particular pad does when untouched, so everything downstream
+    is written once for a pad that behaves. A stick that no longer centres, and
+    a trigger that reads 0 until it first moves, both stop being special cases
+    here rather than in every function that reads one.
+
+    Deflection is measured from `zeros` rather than from the ideal, which is the
+    whole point: the tilt is subtracted, not deadzoned away, so the travel that
+    is left still reaches full speed.
+    """
+    triggers = (layout.left_trigger, layout.right_trigger)
+    out = []
+    for index, value in enumerate(axes):
+        zero = zeros[index] if index < len(zeros) else STICK_REST
+        if index in triggers:
+            travel = (value - zero) / (1.0 - zero) if zero < 1.0 else 0.0
+            out.append(TRIGGER_REST + (1.0 - TRIGGER_REST) * max(0.0, min(1.0, travel)))
+        else:
+            out.append(max(-1.0, min(1.0, value - zero)))
+    return out
+
+
 def from_pad(axes: list[float], buttons: list[bool], config, layout: Layout = LAYOUT) -> list[float]:
     """Map one gamepad reading to the normalised six-vector.
 

dora_teleop/probe.py

@@ -35,7 +35,7 @@ from dora import Node
 
 from dora_teleop import joy, mapping
 from dora_teleop.config import Config
-from dora_teleop.mapping import LAYOUT
+from dora_teleop.mapping import LAYOUT, axis_names
 
 # What each velocity component is, in the order the arm takes them.
 COMPONENTS = (
@@ -50,18 +50,6 @@ COMPONENTS = (
 CLEAR = "\033[H\033[J"
 
 
-def axis_names(layout=LAYOUT) -> dict[int, str]:
-    """Name each axis index the layout uses."""
-    return {
-        layout.left_x: "left stick X",
-        layout.left_y: "left stick Y",
-        layout.left_trigger: "left trigger",
-        layout.right_x: "right stick X",
-        layout.right_y: "right stick Y",
-        layout.right_trigger: "right trigger",
-    }
-
-
 def button_names(layout=LAYOUT) -> dict[int, str]:
     """Name each button index the layout uses."""
     return {
@@ -89,8 +77,15 @@ def bar(value: float, width: int = 21) -> str:
     return "".join(cells)
 
 
-def render(sample, velocity: list[float], config: Config) -> str:
+def render(sample, driven, velocity: list[float], config: Config, mismatch: str | None = None) -> str:
-    """Build the whole display for one reading."""
+    """Build the whole display for one reading.
+
+    ``sample`` is the pad exactly as it was reported, because seeing the raw
+    numbers is the point of this tool. ``driven`` is the same reading after
+    calibration — what the node actually decides from — and ``velocity`` is what
+    it would send. A pad this node refuses reaches here as an absent ``driven``
+    and a ``mismatch`` saying why.
+    """
     lines = ["dora-teleop probe — Ctrl-C to stop", ""]
 
     if not sample.present:
@@ -102,6 +97,18 @@ def render(sample, velocity: list[float], config: Config) -> str:
         ]
         return "\n".join(lines)
 
+    if mismatch:
+        lines += [
+            "  this pad does not match the layout, and the arm will not be driven from it.",
+            "",
+            f"    {mismatch}",
+            "",
+            "  Either something was being held when it was plugged in, or the indices",
+            "  below are numbered differently on this pad. The axes are raw, so they",
+            "  say which: fix `Layout` in dora_teleop/mapping.py to match them.",
+            "",
+        ]
+
     names = axis_names()
     lines.append(f"  axes ({len(sample.axes)} reported)")
     for index, value in enumerate(sample.axes):
@@ -121,7 +128,7 @@ def render(sample, velocity: list[float], config: Config) -> str:
     lines += [
         "",
         f"  moving: {moving}",
-        f"  asking for the arm: {'yes' if mapping.is_active(sample) else 'no'}",
+        f"  asking for the arm: {'yes' if mapping.is_active(driven) else 'no'}",
     ]
     return "\n".join(lines)
 
@@ -137,14 +144,19 @@ def main() -> None:
     args = parser.parse_args()
 
     config = Config.from_env()
+    calibration = joy.Calibration()
     node = Node(args.name)
 
     for event in node:
         if event["type"] != "INPUT" or event["id"] != "joy":
             continue
         sample = joy.decode(event)
-        velocity = mapping.velocity(sample.axes, sample.buttons, config) if sample.present else [0.0] * 6
+        # Through the same calibration the node drives from, so what this prints
-        print(CLEAR + render(sample, velocity, config), flush=True)
+        # stays what the arm would get. What is displayed is still the raw
+        # reading: the indices are what a pad is checked against.
+        driven = calibration.apply(sample)
+        velocity = mapping.velocity(driven.axes, driven.buttons, config) if driven.present else [0.0] * 6
+        print(CLEAR + render(sample, driven, velocity, config, calibration.mismatch), flush=True)
 
 
 if __name__ == "__main__":

tests/conftest.py

@@ -7,6 +7,7 @@ import pytest
 
 from dora_teleop.config import Config
 from dora_teleop.joy import Sample
+from dora_teleop.mapping import LAYOUT, TRIGGER_REST
 
 # The shape `dora-joy` publishes. Declared here rather than inferred for the same
 # reason it is declared there: pyarrow reads an empty `axes` list as `list<null>`,
@@ -66,13 +67,58 @@ def stick(axes=None, buttons=None, present=True):
     values = list(axes or [])
     values += [0.0] * (6 - len(values))
     # A trigger at rest reads -1, and a test that forgot to say so would look
-    # like both triggers held down.
+    # like both triggers held down. Which indices those are comes from the
-    for index in (2, 5):
+    # layout, so a test never has to be edited when a pad renumbers them.
+    for index in (LAYOUT.left_trigger, LAYOUT.right_trigger):
         if axes is None or index >= len(axes):
             values[index] = -1.0
     return Sample(axes=values, buttons=list(buttons or []), present=present)
 
 
+def axes_for(
+    left_x=0.0,
+    left_y=0.0,
+    right_x=0.0,
+    right_y=0.0,
+    left_trigger=TRIGGER_REST,
+    right_trigger=TRIGGER_REST,
+):
+    """Lay out a pad's axes by what each control is rather than by its index.
+
+    A test that numbers its axes says nothing about which control it meant, and
+    goes on passing while meaning something else once a pad renumbers them.
+    """
+    values = [0.0] * 6
+    values[LAYOUT.left_x] = left_x
+    values[LAYOUT.left_y] = left_y
+    values[LAYOUT.right_x] = right_x
+    values[LAYOUT.right_y] = right_y
+    values[LAYOUT.left_trigger] = left_trigger
+    values[LAYOUT.right_trigger] = right_trigger
+    return values
+
+
+def buttons_for(
+    grasp=False, release=False, clear=False, freedrive=False, yaw_left=False, yaw_right=False, estop=False
+):
+    """Lay out a pad's buttons by what each one does rather than by its index.
+
+    The same reason as :func:`axes_for`, and more pressing: an unpressed button
+    reads identically on every pad, so a test that numbers them is the only
+    place a wrong index could ever have been caught.
+    """
+    wanted = {
+        LAYOUT.grasp: grasp,
+        LAYOUT.release: release,
+        LAYOUT.clear: clear,
+        LAYOUT.freedrive: freedrive,
+        LAYOUT.yaw_left: yaw_left,
+        LAYOUT.yaw_right: yaw_right,
+        LAYOUT.estop: estop,
+    }
+    return [wanted.get(index, False) for index in range(max(wanted) + 1)]
+
+
 def joy_message(sample):
     """Build the Arrow row `dora-joy` would publish for one reading."""
     return pa.array(

tests/test_deadman.py

@@ -6,7 +6,7 @@ from dora_teleop import main as loop
 from dora_teleop.arbiter import GAMEPAD, WEB
 from dora_teleop.config import Config
 from dora_teleop.joy import Sample
-from tests.conftest import RecordingNode, input_event, joy_message, stick
+from tests.conftest import RecordingNode, buttons_for, input_event, joy_message, stick
 
 STOP = [0.0] * 6
 PUSHED = [0.0, -1.0]
@@ -47,7 +47,14 @@ def ticks(node, state, count=1):
 
 
 def drive(node, state, sample, count=1):
-    """Hold one reading for `count` ticks, the way a real pad and joy node would."""
+    """Hold one reading for `count` ticks, the way a real pad and joy node would.
+
+    A pad this node has not yet seen at rest is sent one resting reading first,
+    because a real one is always at rest before a thumb reaches it — and the
+    node refuses to drive an arm from a pad it has never seen still.
+    """
+    if not state.joy.calibration.known:
+        send_joy(node, state, stick())
     for _ in range(count):
         send_joy(node, state, sample)
         loop._tick(node, state)
@@ -271,7 +278,7 @@ def test_a_latched_estop_keeps_the_arm_still_under_a_pushed_stick(clock):
     drive(node, state, stick(axes=PUSHED))
     node.clear()
 
-    drive(node, state, stick(axes=PUSHED, buttons=[False] * 6 + [True]))
+    drive(node, state, stick(axes=PUSHED, buttons=buttons_for(estop=True)))
     node.clear()
     jogs = drive(node, state, stick(axes=PUSHED), count=4)
 
@@ -282,11 +289,11 @@ def test_the_pad_can_release_the_latch_it_set(clock):
     # A pad-only dataflow has no console RESET, so a latch nothing on the pad
     # can release would strand the arm until the process was restarted.
     node, state = build()
-    drive(node, state, stick(buttons=[False] * 6 + [True]))
+    drive(node, state, stick(buttons=buttons_for(estop=True)))
     assert state.view.estop is True
     node.clear()
 
-    drive(node, state, stick(buttons=[False, False, True]))
+    drive(node, state, stick(buttons=buttons_for(clear=True)))
 
     assert state.view.estop is False
     assert "clear" in node.services()
@@ -296,20 +303,20 @@ def test_the_latch_is_released_without_holding_the_arm(clock):
     # An idle pad hands the lease back two seconds after the stop it just made,
     # so gating the release on the lease would be a trap.
     node, state = build()
-    drive(node, state, stick(buttons=[False] * 6 + [True]))
+    drive(node, state, stick(buttons=buttons_for(estop=True)))
     state.arbiter.release(GAMEPAD)
     state.arbiter.take(WEB, clock.t)
     node.clear()
 
-    drive(node, state, stick(buttons=[False, False, True]))
+    drive(node, state, stick(buttons=buttons_for(clear=True)))
 
     assert state.view.estop is False
 
 
 def test_the_arm_drives_again_once_the_latch_is_released(clock):
     node, state = build()
-    drive(node, state, stick(buttons=[False] * 6 + [True]))
+    drive(node, state, stick(buttons=buttons_for(estop=True)))
-    drive(node, state, stick(buttons=[False, False, True]))
+    drive(node, state, stick(buttons=buttons_for(clear=True)))
     node.clear()
 
     jogs = drive(node, state, stick(axes=PUSHED))
@@ -322,7 +329,7 @@ def test_a_latched_estop_stops_the_browser_too(clock):
     # through a stop somebody made on the gamepad.
     node, state = build()
     state.clients = 1
-    drive(node, state, stick(buttons=[False] * 6 + [True]))
+    drive(node, state, stick(buttons=buttons_for(estop=True)))
     state.arbiter.release(GAMEPAD)
     state.arbiter.take(WEB, clock.t)
     node.clear()
@@ -337,7 +344,7 @@ def test_the_estop_button_works_without_the_pad_holding_the_arm(clock):
     node, state = build()
     state.arbiter.take(WEB, clock.t)
 
-    drive(node, state, stick(buttons=[False] * 6 + [True]))
+    drive(node, state, stick(buttons=buttons_for(estop=True)))
 
     assert "stop" in node.services()
 
@@ -402,7 +409,7 @@ def test_a_button_asks_in_verbs_as_well_as_in_services(clock):
     # what dora-ulite6 reads until it does.
     node, state = build()
 
-    drive(node, state, stick(buttons=[True]))
+    drive(node, state, stick(buttons=buttons_for(grasp=True)))
 
     assert node.actions() == ["gripper_close"]
     assert "grasp" in node.services()
@@ -410,10 +417,10 @@ def test_a_button_asks_in_verbs_as_well_as_in_services(clock):
 
 def test_every_verb_the_pad_can_ask_for_is_robot_agnostic(clock):
     node, state = build()
-    drive(node, state, stick(buttons=[False, True]))
+    drive(node, state, stick(buttons=buttons_for(release=True)))
-    drive(node, state, stick(buttons=[False] * 3 + [True]))
+    drive(node, state, stick(buttons=buttons_for(freedrive=True)))
-    drive(node, state, stick(buttons=[False] * 6 + [True]))
+    drive(node, state, stick(buttons=buttons_for(estop=True)))
-    drive(node, state, stick(buttons=[False, False, True]))
+    drive(node, state, stick(buttons=buttons_for(clear=True)))
 
     assert node.actions() == ["gripper_open", "freedrive", "stop", "clear"]
 
@@ -426,6 +433,6 @@ def test_the_first_press_after_the_pad_has_been_idle_is_not_dropped(clock):
     node, state = build()
     assert state.arbiter.owner is None
 
-    drive(node, state, stick(buttons=[True]), count=3)
+    drive(node, state, stick(buttons=buttons_for(grasp=True)), count=3)
 
     assert node.actions() == ["gripper_close"]

tests/test_joy.py

@@ -1,9 +1,12 @@
 """The pad as it arrives on the wire, and what silence on that wire means."""
 
 import pyarrow as pa
+import pytest
 
+from dora_teleop import mapping
 from dora_teleop.joy import ABSENT, JoyInput, Sample, decode
-from tests.conftest import input_event, joy_message, stick
+from dora_teleop.mapping import LAYOUT
+from tests.conftest import axes_for, input_event, joy_message, stick
 
 
 def joy_event(sample):
@@ -41,21 +44,107 @@ def test_a_message_with_no_rows_reads_as_absent():
 
 def test_a_fresh_reading_is_handed_back():
     joy = JoyInput(ttl_s=0.2)
-    sample = stick(axes=[0.5])
+    joy.accept(stick(), 100.0)
-    joy.accept(sample, 100.0)
+    joy.accept(stick(axes=[0.5]), 100.0)
 
-    assert joy.read(100.1) is sample
+    assert joy.read(100.1).axes[0] == 0.5
 
 
 def test_a_reading_nobody_is_refreshing_reads_as_absent():
     # dora-joy publishes every tick, so silence means it died — and a dead pad
     # node is indistinguishable from an unplugged pad as far as the arm cares.
     joy = JoyInput(ttl_s=0.2)
+    joy.accept(stick(), 100.0)
     joy.accept(stick(axes=[0.5]), 100.0)
 
     assert joy.read(100.5) is ABSENT
 
 
+# --- the pad this node is actually holding ---
+
+
+def test_a_pad_is_measured_where_it_rests_and_the_tilt_is_subtracted():
+    # A stick that no longer centres is the ordinary kind of worn pad. Its rest
+    # becomes zero, so it asks for nothing until it is actually pushed.
+    joy = JoyInput(ttl_s=0.2)
+    tilted = axes_for(left_x=0.2, left_y=-0.15)
+    joy.accept(stick(axes=tilted), 100.0)
+
+    reading = joy.read(100.1)
+
+    assert reading.present is True
+    assert reading.axes[LAYOUT.left_x] == pytest.approx(0.0)
+    assert reading.axes[LAYOUT.left_y] == pytest.approx(0.0)
+    assert mapping.is_active(reading) is False
+
+
+def test_the_tilt_is_subtracted_rather_than_deadzoned_so_full_travel_remains():
+    joy = JoyInput(ttl_s=0.2)
+    joy.accept(stick(axes=axes_for(left_x=0.2)), 100.0)
+    joy.accept(stick(axes=axes_for(left_x=1.0)), 100.0)
+
+    assert joy.read(100.1).axes[LAYOUT.left_x] == pytest.approx(0.8)
+
+
+def test_a_trigger_that_rests_at_zero_does_not_read_as_half_pressed():
+    # Some pads report a trigger as 0 until it first moves. Read as SDL's -1..1
+    # that is half travel, which would lift the arm on its own.
+    joy = JoyInput(ttl_s=0.2)
+    joy.accept(stick(axes=axes_for(right_trigger=0.0)), 100.0)
+
+    reading = joy.read(100.1)
+
+    assert mapping.trigger(reading.axes[LAYOUT.right_trigger]) == pytest.approx(0.0)
+    assert mapping.is_active(reading) is False
+
+
+def test_a_pad_that_does_not_fit_the_layout_reads_as_absent():
+    # The bug this exists for: a trigger on an index the layout calls a stick
+    # rests at -1.0, which is full deflection — an untouched pad on the desk
+    # asking for full speed, and holding the lease so nothing can take it back.
+    joy = JoyInput(ttl_s=0.2)
+    wrong = list(axes_for())
+    wrong[LAYOUT.right_y] = -1.0
+    joy.accept(stick(axes=wrong), 100.0)
+
+    assert joy.read(100.1) is ABSENT
+    assert "right stick Y" in joy.calibration.mismatch
+
+
+def test_a_pad_held_when_it_appears_is_refused_until_it_is_let_go():
+    # A stick held at startup and a wrongly numbered axis look identical in one
+    # reading, so the safe answer is the same for both: no arm until it rests.
+    joy = JoyInput(ttl_s=0.2)
+    joy.accept(stick(axes=axes_for(left_y=-1.0)), 100.0)
+
+    assert joy.read(100.1) is ABSENT
+
+    joy.accept(stick(), 100.2)
+
+    assert joy.read(100.3).present is True
+
+
+def test_rest_is_not_re_measured_while_the_pad_is_in_use():
+    # Re-measuring under a held stick would record that deflection as centre,
+    # and the arm would then move the moment the operator let go.
+    joy = JoyInput(ttl_s=0.2)
+    joy.accept(stick(), 100.0)
+    joy.accept(stick(axes=axes_for(left_y=-1.0)), 100.1)
+    joy.accept(stick(axes=axes_for(left_y=-1.0)), 100.2)
+
+    assert joy.read(100.3).axes[LAYOUT.left_y] == pytest.approx(-1.0)
+
+
+def test_a_pad_that_goes_away_is_measured_again_when_it_comes_back():
+    # The second pad is not the first one, and may not rest where it did.
+    joy = JoyInput(ttl_s=0.2)
+    joy.accept(stick(axes=axes_for(left_x=0.2)), 100.0)
+    joy.accept(Sample(present=False), 100.1)
+    joy.accept(stick(axes=axes_for(left_x=-0.2)), 100.2)
+
+    assert joy.read(100.3).axes[LAYOUT.left_x] == pytest.approx(0.0)
+
+
 def test_nothing_ever_arriving_reads_as_absent():
     # A dataflow with no `joy` wired at all, which is a browser-only console.
     assert JoyInput(ttl_s=0.2).read(100.0) is ABSENT

tests/test_mapping.py

@@ -3,8 +3,8 @@
 import pytest
 
 from dora_teleop import mapping
-from dora_teleop.mapping import VX, VY, VZ, WX, WY, WZ
+from dora_teleop.mapping import LAYOUT, VX, VY, VZ, WX, WY, WZ
-from tests.conftest import stick
+from tests.conftest import axes_for, buttons_for, stick
 
 
 def test_a_stick_at_rest_asks_for_nothing(config):
@@ -29,15 +29,15 @@ def test_pushing_the_stick_right_moves_the_arm_along_positive_y(config):
 
 
 def test_the_right_trigger_lifts_and_the_left_one_lowers(config):
-    up = mapping.velocity(stick(axes=[0, 0, -1.0, 0, 0, 1.0]).axes, [], config)
+    up = mapping.velocity(axes_for(right_trigger=1.0), [], config)
-    down = mapping.velocity(stick(axes=[0, 0, 1.0, 0, 0, -1.0]).axes, [], config)
+    down = mapping.velocity(axes_for(left_trigger=1.0), [], config)
 
     assert up[VZ] == pytest.approx(config.speed_mm_s)
     assert down[VZ] == pytest.approx(-config.speed_mm_s)
 
 
 def test_both_triggers_together_cancel(config):
-    velocity = mapping.velocity(stick(axes=[0, 0, 1.0, 0, 0, 1.0]).axes, [], config)
+    velocity = mapping.velocity(axes_for(left_trigger=1.0, right_trigger=1.0), [], config)
 
     assert velocity[VZ] == 0.0
 
@@ -51,15 +51,15 @@ def test_an_untouched_trigger_does_not_read_as_half_pressed(config):
 
 
 def test_the_right_stick_turns_the_tool_in_degrees_per_second(config):
-    velocity = mapping.velocity(stick(axes=[0, 0, -1, 1.0, -1.0, -1]).axes, [], config)
+    velocity = mapping.velocity(axes_for(right_x=1.0, right_y=-1.0), [], config)
 
     assert velocity[WX] == pytest.approx(config.speed_deg_s)
     assert velocity[WY] == pytest.approx(config.speed_deg_s)
 
 
 def test_the_bumpers_yaw_in_opposite_directions(config):
-    right = mapping.velocity(stick().axes, [False] * 5 + [True], config)
+    right = mapping.velocity(stick().axes, buttons_for(yaw_right=True), config)
-    left = mapping.velocity(stick().axes, [False] * 4 + [True, False], config)
+    left = mapping.velocity(stick().axes, buttons_for(yaw_left=True), config)
 
     assert right[WZ] == pytest.approx(config.speed_deg_s)
     assert left[WZ] == pytest.approx(-config.speed_deg_s)
@@ -68,7 +68,7 @@ def test_the_bumpers_yaw_in_opposite_directions(config):
 def test_several_axes_move_at_once(config):
     # The whole reason a pad beats the console's four-sector dial: the dial can
     # only ever ask for one axis.
-    velocity = mapping.velocity(stick(axes=[1.0, -1.0, -1, 0, 0, 1.0]).axes, [], config)
+    velocity = mapping.velocity(axes_for(left_x=1.0, left_y=-1.0, right_trigger=1.0), [], config)
 
     assert velocity[VX] > 0
     assert velocity[VY] > 0
@@ -129,14 +129,90 @@ def test_a_nudged_stick_is_asking_for_the_arm():
 
 
 def test_a_held_trigger_is_asking_for_the_arm():
-    assert mapping.is_active(stick(axes=[0.0, 0.0, 1.0])) is True
+    assert mapping.is_active(stick(axes=axes_for(left_trigger=1.0))) is True
 
 
 def test_a_held_button_is_asking_for_the_arm():
-    assert mapping.is_active(stick(buttons=[True])) is True
+    assert mapping.is_active(stick(buttons=buttons_for(grasp=True))) is True
 
 
 def test_an_axis_the_layout_does_not_use_still_counts():
     # A pad reporting its d-pad as two extra axes is asking for the arm when
     # somebody presses it, whether or not this node maps it to anything.
-    assert mapping.is_active(stick(axes=[0.0, 0.0, -1.0, 0.0, 0.0, -1.0, 1.0])) is True
+    assert mapping.is_active(stick(axes=axes_for() + [1.0])) is True
+
+
+# --- reading a pad that is not the layout's ideal one ---
+
+
+def test_a_resting_pad_measures_as_resting():
+    zeros = mapping.rest_zeros(axes_for())
+
+    assert zeros == axes_for()
+
+
+def test_a_small_tilt_is_accepted_as_rest():
+    # A worn stick is an ordinary pad, not a broken one.
+    assert mapping.rest_zeros(axes_for(left_x=0.2, right_y=-0.2))
+
+
+def test_an_axis_resting_where_the_layout_says_it_cannot_is_refused():
+    wrong = axes_for()
+    wrong[LAYOUT.right_y] = -1.0
+
+    with pytest.raises(ValueError, match="right stick Y"):
+        mapping.rest_zeros(wrong)
+
+
+def test_the_refusal_names_the_index_and_what_it_read():
+    # It is read off a terminal by somebody deciding what to put in `Layout`.
+    wrong = axes_for()
+    wrong[LAYOUT.right_x] = 0.9
+
+    with pytest.raises(ValueError, match=r"axis 2 rests at \+0.900"):
+        mapping.rest_zeros(wrong)
+
+
+def test_a_trigger_resting_at_either_end_is_accepted():
+    # -1 is what SDL reports; 0 is what some pads report until it first moves.
+    assert mapping.rest_zeros(axes_for(left_trigger=-1.0, right_trigger=0.0))
+
+
+def test_an_axis_the_layout_does_not_use_still_has_to_rest():
+    # A d-pad reported as two extra axes counts towards `is_active`, so one
+    # stuck at an extreme would hold the arm as surely as a named one.
+    with pytest.raises(ValueError, match="axis 6"):
+        mapping.rest_zeros(axes_for() + [1.0])
+
+
+def test_canonical_puts_a_tilted_stick_back_at_centre():
+    tilted = axes_for(left_x=0.2)
+
+    canonical = mapping.canonical(tilted, mapping.rest_zeros(tilted))
+
+    assert canonical[LAYOUT.left_x] == pytest.approx(0.0)
+
+
+def test_canonical_puts_a_trigger_back_at_its_own_rest():
+    zeroed = axes_for(left_trigger=0.0, right_trigger=0.0)
+
+    canonical = mapping.canonical(zeroed, mapping.rest_zeros(zeroed))
+
+    assert canonical[LAYOUT.left_trigger] == pytest.approx(mapping.TRIGGER_REST)
+    assert canonical[LAYOUT.right_trigger] == pytest.approx(mapping.TRIGGER_REST)
+
+
+def test_a_trigger_measured_from_zero_still_reaches_full_travel():
+    zeros = mapping.rest_zeros(axes_for(right_trigger=0.0))
+
+    canonical = mapping.canonical(axes_for(right_trigger=1.0), zeros)
+
+    assert mapping.trigger(canonical[LAYOUT.right_trigger]) == pytest.approx(1.0)
+
+
+def test_a_calibrated_pad_at_rest_asks_for_nothing(config):
+    tilted = axes_for(left_x=0.2, left_y=-0.15, right_trigger=0.0)
+
+    canonical = mapping.canonical(tilted, mapping.rest_zeros(tilted))
+
+    assert mapping.velocity(canonical, [], config) == [0.0] * 6

tests/test_probe.py

@@ -2,7 +2,7 @@
 
 from dora_teleop import mapping, probe
 from dora_teleop.joy import Sample
-from tests.conftest import stick
+from tests.conftest import buttons_for, stick
 
 
 def test_a_centred_stick_puts_the_marker_in_the_middle():
@@ -28,7 +28,7 @@ def test_a_reading_beyond_full_deflection_stays_on_the_bar():
 def test_no_gamepad_says_so_and_says_the_arm_is_being_held_still(config):
     # Absence covers both "no pad" and "dora-joy stopped sending", and the probe
     # must not imply the arm is waiting on something it is not.
-    drawn = probe.render(Sample(present=False), [0.0] * 6, config)
+    drawn = probe.render(Sample(present=False), Sample(present=False), [0.0] * 6, config)
 
     assert "no gamepad" in drawn
     assert "hold still" in drawn
@@ -39,30 +39,31 @@ def test_every_axis_the_pad_reports_is_shown_even_the_unused_ones(config):
     # for, so nothing may be hidden.
     sample = Sample(axes=[0.0] * 8, buttons=[], present=True)
 
-    drawn = probe.render(sample, [0.0] * 6, config)
+    drawn = probe.render(sample, sample, [0.0] * 6, config)
 
     assert "unused" in drawn
     assert "axes (8 reported)" in drawn
 
 
 def test_a_pressed_button_is_named(config):
-    sample = stick(buttons=[True])
+    sample = stick(buttons=buttons_for(grasp=True))
 
-    drawn = probe.render(sample, [0.0] * 6, config)
+    drawn = probe.render(sample, sample, [0.0] * 6, config)
 
     assert "grasp" in drawn
 
 
 def test_nothing_pressed_says_so_rather_than_showing_an_empty_line(config):
-    drawn = probe.render(stick(), [0.0] * 6, config)
+    drawn = probe.render(stick(), stick(), [0.0] * 6, config)
 
     assert "none pressed" in drawn
 
 
 def test_the_velocity_is_shown_in_the_units_the_arm_takes(config):
-    velocity = mapping.velocity(stick(axes=[0.0, -1.0]).axes, [], config)
+    pushed = stick(axes=[0.0, -1.0])
+    velocity = mapping.velocity(pushed.axes, [], config)
 
-    drawn = probe.render(stick(axes=[0.0, -1.0]), velocity, config)
+    drawn = probe.render(pushed, pushed, velocity, config)
 
     assert "mm/s" in drawn
     assert "deg/s" in drawn
@@ -70,13 +71,14 @@ def test_the_velocity_is_shown_in_the_units_the_arm_takes(config):
 
 
 def test_a_still_arm_is_reported_as_still(config):
-    drawn = probe.render(stick(), [0.0] * 6, config)
+    drawn = probe.render(stick(), stick(), [0.0] * 6, config)
 
     assert "told to hold still" in drawn
 
 
 def test_a_moving_arm_is_reported_as_moving(config):
-    drawn = probe.render(stick(axes=[0.0, -1.0]), [10.0, 0, 0, 0, 0, 0], config)
+    pushed = stick(axes=[0.0, -1.0])
+    drawn = probe.render(pushed, pushed, [10.0, 0, 0, 0, 0, 0], config)
 
     assert "moving: yes" in drawn
 
@@ -84,5 +86,30 @@ def test_a_moving_arm_is_reported_as_moving(config):
 def test_whether_the_pad_is_asking_for_the_arm_is_shown(config):
     # The probe is where a resting trigger claiming the arm would have been
     # visible, so it says what `is_active` thinks rather than leaving it implied.
-    assert "asking for the arm: no" in probe.render(stick(), [0.0] * 6, config)
+    rest, pushed = stick(), stick(axes=[0.0, -1.0])
-    assert "asking for the arm: yes" in probe.render(stick(axes=[0.0, -1.0]), [0.0] * 6, config)
+
+    assert "asking for the arm: no" in probe.render(rest, rest, [0.0] * 6, config)
+    assert "asking for the arm: yes" in probe.render(pushed, pushed, [0.0] * 6, config)
+
+
+def test_a_pad_that_does_not_fit_the_layout_is_called_out(config):
+    # This is the tool for exactly that pad, so the reason goes on the screen
+    # rather than only in the node's log.
+    wrong = stick(axes=[0.0, 0.0, 0.0, -1.0, -1.0, -1.0])
+    mismatch = "axis 3 rests at -1.000, which is not where right stick Y sits"
+
+    drawn = probe.render(wrong, Sample(present=False), [0.0] * 6, config, mismatch)
+
+    assert "does not match the layout" in drawn
+    assert mismatch in drawn
+    assert "asking for the arm: no" in drawn
+
+
+def test_the_raw_axes_are_still_shown_for_a_pad_that_does_not_fit(config):
+    # The numbers are the evidence: they are what `Layout` gets corrected from.
+    wrong = stick(axes=[0.0, 0.0, 0.0, -1.0, -1.0, -1.0])
+
+    drawn = probe.render(wrong, Sample(present=False), [0.0] * 6, config, "axis 3")
+
+    assert "-1.000" in drawn
+    assert "axes (6 reported)" in drawn