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

tools/moss_floor_probe.py: measure the static-friction floor of the tracks

metrox4db3f71afb55
diff --git a/benches/bench_moss_floor_probe_cold.py b/benches/bench_moss_floor_probe_cold.py
new file mode 100644
index 0000000..ab09aa8
--- /dev/null
+++ b/benches/bench_moss_floor_probe_cold.py
@@ -0,0 +1,105 @@
+"""Cold bench: tools/moss_floor_probe.py, the static-friction floor finder.
+
+Known input -> known output: a table of tick deltas gives a known floor; a
+fake firmware whose encoders only turn from 35 % makes the probe report 35,
+stop at that step, and leave the base stopped. No hardware, no dimOS.
+
+Run: PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=. python3 benches/bench_moss_floor_probe_cold.py
+"""
+import importlib.util
+import os
+import re
+import sys
+from pathlib import Path
+
+ROOT = Path(__file__).resolve().parent.parent
+sys.path.insert(0, str(ROOT))
+
+from moss_dimos import moss_mock # noqa: E402
+from moss_dimos.moss_adapter import MossBaseAdapter, MossGeometry # noqa: E402
+
+spec = importlib.util.spec_from_file_location("moss_floor_probe", ROOT / "tools" / "moss_floor_probe.py")
+fp = importlib.util.module_from_spec(spec)
+spec.loader.exec_module(fp)
+
+OK = KO = 0
+
+
+def check(cond, label, detail=""):
+ global OK, KO
+ if cond:
+ OK += 1
+ else:
+ KO += 1
+ print((" OK " if cond else " KO ") + label + (f" - {detail}" if detail and not cond else ""))
+
+
+print("A. plan(start, stop, step)")
+check(fp.plan(20, 60, 5) == [20, 25, 30, 35, 40, 45, 50, 55, 60], "20..60 by 5 -> nine steps")
+check(fp.plan(0, 200, 50) == [1, 51], "clamped to 1..100: 0..200 by 50 -> 1, 51")
+check(fp.plan(60, 20, 5) == [], "start above stop -> no step")
+try:
+ fp.plan(20, 60, 0)
+ check(False, "step 0 -> ValueError")
+except ValueError:
+ check(True, "step 0 -> ValueError")
+
+print("B. judge(rows, min_ticks)")
+rows = [{"pct": 20, "left": 0, "right": 0}, {"pct": 25, "left": 48, "right": 3},
+ {"pct": 30, "left": 90, "right": 12}, {"pct": 35, "left": 120, "right": 95}, {"pct": 40, "left": 150, "right": 140}]
+check(fp.judge(rows, 20) == 35, "left turns from 25 %, right from 35 % -> floor 35 (both sides)")
+check(fp.judge(rows, 100) == 40, "a stricter motion threshold (100 ticks) -> 40")
+check(fp.judge(rows[:3], 20) is None, "no row with both sides moving -> None")
+check(fp.judge([{"pct": 30, "left": -60, "right": -70}], 20) == 30, "reverse ticks count too (absolute)")
+
+print("C. the probe on a fake firmware whose encoders turn from 35 % (left from 25 %)")
+
+
+class Moving(moss_mock.FakeMossFirmware):
+ """The stock fake never turns its encoders; this one does, per telemetry line,
+ once the applied percent is high enough: left from 25 %, right from 35 %."""
+
+ def _telemetry_line(self):
+ if self.mode == moss_mock.DRIVE:
+ if abs(self.left) >= 25:
+ self.ticks[0] += 60
+ if abs(self.right) >= 35:
+ self.ticks[1] += 60
+ return super()._telemetry_line()
+
+
+fake = Moving(auto_time=True)
+factory = lambda: fake # noqa: E731
+factory.is_mock = True
+adapter = MossBaseAdapter(port_factory=factory, geometry=MossGeometry(mps_at_limit=1.0, max_pct=100, min_pct=0))
+check(adapter.connect(), "the probe's adapter connects to the fake")
+said = []
+rows = fp.run(adapter, [20, 25, 30, 35, 40], hold=0.6, settle=0.4, min_ticks=20, stop_at_first=True, say=said.append)
+adapter.disconnect()
+check([r["pct"] for r in rows] == [20, 25, 30, 35], f"stops at the first step where both move: steps driven {[r['pct'] for r in rows]}")
+check(rows[-1]["left"] >= 20 and rows[-1]["right"] >= 20, f"the 35 % row shows both sides moving: {rows[-1]}")
+check(rows[1]["left"] >= 20 and abs(rows[1]["right"]) < 20, f"the 25 % row shows the left side only: {rows[1]}")
+check(fp.judge(rows, 20) == 35, "judge -> 35")
+check(fake.mode == moss_mock.OFF, f"the base is stopped after the run (fake mode {fake.mode})")
+check(any("BOTH" in s for s in said) and any("left" in s for s in said), "the table names the sides that moved")
+
+print("D. main(): dry run, then the failure path through the stock fake")
+rc = fp.main([])
+check(rc == 0, "no --go -> plan only, exit 0")
+os.environ["MOSS_MOCK_BUS"] = "1"
+rc = fp.main(["--go", "--mock", "--from", "30", "--to", "40", "--step", "5", "--hold", "0.3", "--settle", "0.2"])
+check(rc == 1, "the stock fake never turns its encoders -> 'no step turned both tracks', exit 1")
+moss_mock.FakeMossFirmware, stock = Moving, moss_mock.FakeMossFirmware
+try:
+ rc = fp.main(["--go", "--mock", "--from", "30", "--to", "40", "--step", "5", "--hold", "0.4", "--settle", "0.3"])
+ check(rc == 0, "the moving fake -> floor found, exit 0")
+finally:
+ moss_mock.FakeMossFirmware = stock
+ os.environ.pop("MOSS_MOCK_BUS", None)
+check(re.search(fp.STACK_RE, "/home/x/moss/.venv/bin/dimos --rerun-open none run moss-dimos.moss-nav --daemon") is not None,
+ "the running-stack pattern matches a real launch line")
+check(re.search(fp.STACK_RE, "pgrep -f [b]in/dimos( [^ ]+)* run( |$)") is None, "...and not its own pgrep")
+
+print(f"\n{OK} OK, {KO} KO")
+print("TEST PASSED" if KO == 0 else "TEST FAILED")
+sys.exit(1 if KO else 0)
diff --git a/docs/base.md b/docs/base.md
index 39d5053..1e25cd7 100644
--- a/docs/base.md
+++ b/docs/base.md
@@ -78,6 +78,7 @@ env override, so a run can be re-scaled without a code change:
| `MOSS_TRACK_M` | distance between the two track centrelines (0.266 m from the CAD) | tape measure, sprocket centre to sprocket centre |
| `MOSS_MPS_AT_LIMIT` | floor speed at `MOSS_MAX_PCT` (0.45 m/s placeholder) | full stick (or `drive 100 100`) for 3 s on the floor, tape measure, divide |
| `MOSS_TICKS_PER_M` | encoder ticks per metre of track travel (unset) | `zero`, push or drive the rover exactly 1 m, read `ticks` in a `status` line, average the two sides |
+| `MOSS_MIN_PCT` | static-friction floor: the lowest percent that turns both tracks (0 = off) | `tools/moss_floor_probe.py --go` with the stack down, the rover on blocks or with a clear metre ahead: it steps the percent up, reads the encoders around each step and prints the value to export |
Until `MOSS_TICKS_PER_M` is set, `read_odometry()` returns `None` and
`read_velocities()` returns the commanded twist: an honest "no measurement"
diff --git a/tools/moss_floor_probe.py b/tools/moss_floor_probe.py
new file mode 100755
index 0000000..8e22687
--- /dev/null
+++ b/tools/moss_floor_probe.py
@@ -0,0 +1,173 @@
+#!/usr/bin/env python3
+"""Find the static-friction floor of the tracks: the lowest PWM percent that
+turns BOTH tracks. The planner's slowest commands map to a few percent, below
+what the geared motors need to start, so the base adapter raises any non-zero
+command to MOSS_MIN_PCT; this tool measures that number.
+
+It steps the percent up (both tracks forward), holds each step, reads the
+encoder ticks before and after, and reports the first step where both sides
+moved. It prints the value to export; it writes nothing.
+
+Run it with the stack DOWN (it opens the motor board itself), the rover on
+blocks or with a clear metre ahead: every step rolls it forward a little.
+
+ tools/moss_floor_probe.py # the plan only, nothing moves
+ tools/moss_floor_probe.py --go # drive the steps
+ tools/moss_floor_probe.py --go --all # keep stepping past the first success
+
+Options: --from 20 --to 60 --step 5 (percent), --hold 1.0 (s of drive per
+step), --settle 1.0 (s of stop before reading the ticks), --min-ticks 20
+(encoder ticks that count as motion), --port <by-id path> (default: the QT Py
+by-id link or MOSS_PORT), --mock (the fake firmware, no motor).
+"""
+from __future__ import annotations
+
+import argparse
+import os
+import re
+import subprocess
+import sys
+import time
+from pathlib import Path
+
+ROOT = Path(__file__).resolve().parent.parent
+sys.path.insert(0, str(ROOT))
+
+from moss_dimos.moss_adapter import MossBaseAdapter, MossGeometry # noqa: E402
+
+STACK_RE = r"[b]in/dimos( [^ ]+)* run( |$)"
+TELEMETRY_WAIT_S = 3.0
+
+
+def plan(start: int, stop: int, step: int) -> list[int]:
+ """The percents to try, start..stop inclusive, clamped to 1..100."""
+ if step <= 0:
+ raise ValueError("step must be positive")
+ start, stop = max(1, min(100, start)), max(1, min(100, stop))
+ return list(range(start, stop + 1, step)) if start <= stop else []
+
+
+def judge(rows: list[dict], min_ticks: int) -> int | None:
+ """rows: [{"pct": int, "left": ticks moved, "right": ticks moved}, ...] in
+ ascending percent. The floor is the first percent where BOTH sides moved by
+ at least min_ticks; None when no row qualifies."""
+ for r in rows:
+ if abs(int(r["left"])) >= min_ticks and abs(int(r["right"])) >= min_ticks:
+ return int(r["pct"])
+ return None
+
+
+def stack_running() -> bool:
+ try:
+ out = subprocess.run(["pgrep", "-f", STACK_RE], capture_output=True, text=True, timeout=5).stdout
+ except Exception: # noqa: BLE001
+ return False
+ return bool(out.strip())
+
+
+def _ticks(adapter: MossBaseAdapter) -> list[int] | None:
+ t = adapter.telemetry
+ if not t or "ticks" not in t:
+ return None
+ return [int(t["ticks"][0]), int(t["ticks"][1])]
+
+
+def _fresh_ticks(adapter: MossBaseAdapter, timeout: float) -> list[int] | None:
+ """Ticks from a telemetry line younger than the call."""
+ deadline = time.monotonic() + timeout
+ t0 = time.monotonic()
+ while time.monotonic() < deadline:
+ age = adapter.telemetry_age_s
+ if age is not None and age < (time.monotonic() - t0) + 0.05:
+ return _ticks(adapter)
+ time.sleep(0.05)
+ return _ticks(adapter)
+
+
+def run(adapter: MossBaseAdapter, pcts: list[int], hold: float, settle: float,
+ min_ticks: int, stop_at_first: bool, say=print) -> list[dict]:
+ """Drive each step and read the ticks around it. Returns the rows; the
+ adapter is stopped before returning, whatever happens."""
+ rows: list[dict] = []
+ try:
+ for pct in pcts:
+ before = _fresh_ticks(adapter, TELEMETRY_WAIT_S)
+ if before is None:
+ say("no telemetry from the firmware: is it the QT Py port, and is the firmware flashed?")
+ break
+ # the adapter refuses a non-zero drive until the firmware is armed through it
+ if not adapter.read_enabled() and not adapter.write_enable(True):
+ say("the firmware refused 'arm' (see the lines above)")
+ break
+ t_end = time.monotonic() + hold
+ ok = True
+ while time.monotonic() < t_end:
+ # geometry: mps_at_limit 1.0 and max_pct 100, so vx 0.35 m/s = 35 %
+ ok = adapter.write_velocities([pct / 100.0, 0.0, 0.0]) and ok
+ time.sleep(0.2)
+ adapter.write_stop()
+ time.sleep(settle)
+ after = _fresh_ticks(adapter, TELEMETRY_WAIT_S) or before
+ row = {"pct": pct, "left": after[0] - before[0], "right": after[1] - before[1], "sent": ok}
+ rows.append(row)
+ mark = "BOTH" if abs(row["left"]) >= min_ticks and abs(row["right"]) >= min_ticks else (
+ "left" if abs(row["left"]) >= min_ticks else ("right" if abs(row["right"]) >= min_ticks else "-"))
+ say(f" {pct:3d} % left {row['left']:+6d} right {row['right']:+6d} ticks -> {mark}"
+ + ("" if ok else " (a drive was refused)"))
+ if stop_at_first and mark == "BOTH":
+ break
+ finally:
+ try:
+ adapter.write_stop()
+ adapter.write_enable(False) # 'stop': disarmed, nothing moves after this tool
+ except Exception: # noqa: BLE001
+ pass
+ return rows
+
+
+def main(argv=None) -> int:
+ ap = argparse.ArgumentParser(description=__doc__.split("\n\n")[0])
+ ap.add_argument("--go", action="store_true", help="drive the steps (without it: print the plan)")
+ ap.add_argument("--all", action="store_true", help="keep stepping past the first success")
+ ap.add_argument("--from", dest="start", type=int, default=20)
+ ap.add_argument("--to", dest="stop", type=int, default=60)
+ ap.add_argument("--step", type=int, default=5)
+ ap.add_argument("--hold", type=float, default=1.0)
+ ap.add_argument("--settle", type=float, default=1.0)
+ ap.add_argument("--min-ticks", type=int, default=20)
+ ap.add_argument("--port", default=None)
+ ap.add_argument("--mock", action="store_true", help="the fake firmware, no motor")
+ a = ap.parse_args(argv)
+
+ pcts = plan(a.start, a.stop, a.step)
+ print(f"floor probe: {len(pcts)} steps {pcts[0] if pcts else '-'}..{pcts[-1] if pcts else '-'} % "
+ f"(hold {a.hold:.1f} s, settle {a.settle:.1f} s, motion = {a.min_ticks} ticks)")
+ if not a.go:
+ print("nothing moves without --go. Put the rover on blocks or give it a clear metre ahead.")
+ return 0
+ if not a.mock and stack_running():
+ print("a dimOS stack is running: stop it first (tools/moss-fly.sh stop), this tool opens the motor board itself")
+ return 2
+ kw = {"geometry": MossGeometry(mps_at_limit=1.0, max_pct=100, min_pct=0)}
+ if a.mock:
+ os.environ["MOSS_MOCK_BUS"] = "1"
+ elif a.port:
+ kw["address"] = a.port
+ adapter = MossBaseAdapter(**kw)
+ if not adapter.connect():
+ print("cannot connect to the firmware (see the lines above)")
+ return 2
+ try:
+ rows = run(adapter, pcts, a.hold, a.settle, a.min_ticks, stop_at_first=not a.all)
+ finally:
+ adapter.disconnect()
+ floor = judge(rows, a.min_ticks)
+ if floor is None:
+ print(f"no step up to {pcts[-1] if pcts else '-'} % turned both tracks: raise --to, or check the drive line")
+ return 1
+ print(f"both tracks turn from {floor} %: export MOSS_MIN_PCT={floor} (or launch with MOSS_MIN_PCT={floor} ...)")
+ return 0
+
+
+if __name__ == "__main__":
+ sys.exit(main())