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.pynew file mode 100644index 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.mdindex 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.pynew file mode 100755index 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())