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

base: the floor keeps the curvature, every command is scaled for the pack voltage, and a pivot with one frozen track turns on the other track instead of counting as a contact

metroxa458bc849a24
diff --git a/benches/bench_moss_adapter_cold.py b/benches/bench_moss_adapter_cold.py
index c9d75e7..98f8b58 100644
--- a/benches/bench_moss_adapter_cold.py
+++ b/benches/bench_moss_adapter_cold.py
@@ -49,6 +49,7 @@ def settle(s=0.06):
# ── 0. gamepad scale: full stick = the base's limit, not the generic 0.6 m/s ──
os.environ["MOSS_PULSE_MS"] = "0" # sections 0-12: the plain mapping and keepalive; pulsing has its own section
+os.environ["MOSS_BUS_REF_MV"] = "0" # the fake reports 11.6 V: sections 0-14 compare raw percents; the bus compensation has its own section (15)
print("0. teleop_speeds / teleop_envelope (track 0.30 m, 0.45 m/s at 100 %, turn gain 2)")
os.environ.pop("MOSS_TURN_GAIN", None)
G100 = MossGeometry(track_m=0.30, mps_at_limit=0.45, max_pct=100, min_pct=0) # floor off: the mapping itself
@@ -548,6 +549,79 @@ check(not sb.stalled and sb.stall_count == 0 and fw10.mode == DRIVE, f"no stall
sb.write_stop(); sb.disconnect()
os.environ.pop("MOSS_PULSE_MS", None)
+# ── 15. the floor keeps the curvature, the bus compensation, the one-track pivot ─────
+print("15. floor_tracks keeps the curvature; bus_gain / bus_scaled; a pivot with one frozen track turns on the other")
+from moss_dimos.moss_adapter import (floor_tracks, bus_gain, bus_scaled, bus_ref_mv, one_track_pivot_on, # noqa: E402
+ BUS_GAIN_MAX, BUS_REF_MV_DEFAULT)
+check(floor_tracks((57, 32), 50, 100) == (89, 50), f"arc 57/32 under a floor of 50 -> 89/50 (scaled up whole, ratio kept): {floor_tracks((57, 32), 50, 100)}")
+check(floor_tracks((32, 57), 50, 100) == (50, 89), "...and mirrored 32/57 -> 50/89")
+check(floor_tracks((-57, -32), 50, 100) == (-89, -50), "...in reverse -89/-50 (signs kept)")
+check(floor_tracks((57, 10), 50, 100) == (100, 0), f"arc 57/10: the outer track would pass 100 -> 100/0, a one-track turn in the asked direction: {floor_tracks((57, 10), 50, 100)}")
+check(floor_tracks((4, 4), 50, 100) == (50, 50), "a slow straight 4/4 -> 50/50")
+check(floor_tracks((0, 30), 50, 100) == (0, 50) and floor_tracks((-30, 30), 50, 100) == (-50, 50), "a lone track and a pivot are raised to the floor as before")
+check(floor_tracks((60, 55), 50, 100) == (60, 55) and floor_tracks((57, 32), 0, 100) == (57, 32) and floor_tracks((0, 0), 50, 100) == (0, 0),
+ "above the floor, floor off, or a stop: unchanged")
+G50 = MossGeometry(track_m=0.30, mps_at_limit=0.45, max_pct=100, min_pct=50)
+check(twist_to_tracks(0.2, -0.385, G50) == (89, 50), f"the planner's slow right arc vx=0.2 wz=-0.385 (57/32 raw) -> 89/50: {twist_to_tracks(0.2, -0.385, G50)}")
+check(twist_to_tracks(0.15, -0.5, G50) == (100, 0), f"a tighter one vx=0.15 wz=-0.5 (50/17 raw) -> 100/0: {twist_to_tracks(0.15, -0.5, G50)}")
+check(abs(bus_gain(10500, 12400) - 12400 / 10500) < 1e-9 and bus_gain(12600, 12400) == 1.0 and bus_gain(7000, 12400) == BUS_GAIN_MAX,
+ f"bus_gain: 10.5 V -> x{bus_gain(10500, 12400):.3f}, a full pack -> x1, 7 V -> capped x{BUS_GAIN_MAX}")
+check(bus_gain(None, 12400) == 1.0 and bus_gain("n/a", 12400) == 1.0 and bus_gain(10500, 0) == 1.0 and bus_gain(30000, 12400) == 1.0 and bus_gain(0, 12400) == 1.0,
+ "bus_gain: no reading, text, no reference, or an insane reading -> x1")
+check(bus_scaled((56, 50), 12400 / 10500, 100) == (66, 59) and bus_scaled((100, -100), 1.2, 100) == (100, -100) and bus_scaled((0, 50), 1.5, 100) == (0, 75),
+ f"bus_scaled: 56/50 at 10.5 V -> {bus_scaled((56, 50), 12400 / 10500, 100)}, a pivot stays at the limit, a 0 stays 0")
+check(bus_ref_mv() == 0, "MOSS_BUS_REF_MV=0 (this bench's prelude) -> 0")
+os.environ["MOSS_BUS_REF_MV"] = "12400"
+check(bus_ref_mv() == 12400 and one_track_pivot_on() is True, "MOSS_BUS_REF_MV=12400 -> 12400; MOSS_ONE_TRACK_PIVOT unset -> on")
+os.environ.pop("MOSS_BUS_REF_MV", None)
+check(bus_ref_mv() == BUS_REF_MV_DEFAULT == 12400, "MOSS_BUS_REF_MV unset -> 12400 (a full 3S pack, where the floor was measured)")
+os.environ["MOSS_BUS_REF_MV"] = "12400"
+fw15 = Turning(auto_time=True)
+f15 = lambda: fw15 # noqa: E731
+f15.is_mock = True
+se = MossBaseAdapter(dof=3, port_factory=f15, geometry=MossGeometry(track_m=0.30, mps_at_limit=0.45, max_pct=100, min_pct=0))
+check(se.connect() and se.write_enable(True) and se.telemetry.get("bus_mV") == 11600, "adapter on the fake (11.6 V on the bus), armed")
+check(se.write_velocities([0.252, 0.0, 0.0]) and se._want == (56, 56) and se.target_pct == (60, 60) and fw15.commands[-1] == "drive 60 60",
+ f"vx=0.252 (56 %) at 11.6 V with a 12.4 V reference -> drive 60 60 on the wire, the commander's 56/56 kept: want {se._want}, wire {se.target_pct}, {fw15.commands[-1]}")
+check(se.write_velocities([0.45, 0.0, 0.0]) and se.target_pct == (100, 100), "100 % stays 100 % (capped)")
+se.write_stop(); se.disconnect()
+os.environ["MOSS_BUS_REF_MV"] = "0"
+
+# a fake whose LEFT track turns and whose right track never does: a pivot to the right (right in reverse)
+class LeftOnly(FakeMossFirmware):
+ def _telemetry_line(self):
+ if self.mode == DRIVE:
+ self.ticks[0] += 80
+ return super()._telemetry_line()
+
+
+for env_on, label in (("1", "MOSS_ONE_TRACK_PIVOT on"), ("0", "MOSS_ONE_TRACK_PIVOT=0")):
+ os.environ["MOSS_ONE_TRACK_PIVOT"] = env_on
+ fw16 = LeftOnly(auto_time=True)
+ f16 = lambda fw=fw16: fw # noqa: E731
+ f16.is_mock = True
+ sf = MossBaseAdapter(dof=3, port_factory=f16, geometry=MossGeometry(track_m=0.30, mps_at_limit=0.45, max_pct=100, min_pct=0))
+ sf.connect(); sf.write_enable(True)
+ t_start = time.monotonic()
+ while time.monotonic() - t_start < 2 * STALL_S + 1.0 and not sf.stalled and not sf._dead_pivots:
+ sf.write_velocities([0.0, 0.0, -2.0]); settle(0.1) # a pivot to the right: L=+100 R=-100
+ if env_on == "1":
+ check(sf._dead_pivots == {("R", -1)} and sf.stall_count == 0 and not sf.stalled,
+ f"{label}: the right track frozen in reverse while the left turns -> remembered as dead (R, reverse), no stall (count {sf.stall_count}, dead {sf._dead_pivots})")
+ check(sf._want == (100, 0) and fw16.commands[-1] == "drive 100 0",
+ f"...the pivot goes on on the left track, the right braked: want {sf._want}, wire {fw16.commands[-1]}")
+ check(sf.write_velocities([0.0, 0.0, -2.0]) and sf._want == (100, 0) and sf.target_pct == (100, 0),
+ f"...the planner's next pivot to the right is rewritten at once: {sf._want}")
+ check(sf.write_velocities([0.0, 0.0, 2.0]) and sf._want == (-100, 100),
+ f"...a pivot to the LEFT (right track forward) is untouched: {sf._want}")
+ check(sf.write_velocities([0.2, 0.0, 0.0]) and sf._want == (44, 44), f"...a straight drive is untouched: {sf._want}")
+ else:
+ check(sf.stalled and sf.stall_count == 1 and not sf._dead_pivots,
+ f"{label}: the same pivot is a stall, as before (count {sf.stall_count})")
+ sf.write_stop(); sf.disconnect()
+os.environ.pop("MOSS_ONE_TRACK_PIVOT", None)
+os.environ.pop("MOSS_BUS_REF_MV", None)
+
print(f"{OK} OK, {KO} KO")
print("TEST PASSED" if KO == 0 else "TEST FAILED")
sys.exit(1 if KO else 0)
diff --git a/benches/bench_moss_floor_probe_cold.py b/benches/bench_moss_floor_probe_cold.py
index 0b0342f..affed92 100644
--- a/benches/bench_moss_floor_probe_cold.py
+++ b/benches/bench_moss_floor_probe_cold.py
@@ -97,6 +97,10 @@ try:
finally:
moss_mock.FakeMossFirmware = stock
os.environ.pop("MOSS_MOCK_BUS", None)
+check(os.environ.get("MOSS_PIVOT_PCT") == "0" and os.environ.get("MOSS_BUS_REF_MV") == "0" and os.environ.get("MOSS_ONE_TRACK_PIVOT") == "0",
+ "a --go run measures the raw percent: pivots as asked, no bus compensation, no one-track pivot rewrite")
+for _v in ("MOSS_PIVOT_PCT", "MOSS_BUS_REF_MV", "MOSS_ONE_TRACK_PIVOT"):
+ os.environ.pop(_v, 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")
diff --git a/docs/base.md b/docs/base.md
index fe46e37..4fa60f1 100644
--- a/docs/base.md
+++ b/docs/base.md
@@ -79,14 +79,17 @@ 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 (100 on MOSS, 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 |
+| `MOSS_MIN_PCT` | static-friction floor: the lowest percent that turns both tracks (50 on MOSS: 40 measured on a full pack, 10 of margin; 0 = off). An arc whose inner track sits under the floor is scaled up whole, so the curvature the planner asked for survives; raising only the inner track would straighten it | `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 |
+
+| `MOSS_BUS_REF_MV` | the bus voltage the floor and the speeds were measured at (12400 = a full 3S pack; 0 = off). A percent is a duty of the bus, so a pack at 10.5 V under load puts 15 % less voltage on the motors for the same command and the tracks creep or freeze: every command is scaled by reference/bus (never below 1, at most 1.5, capped at the limit) | read `bus_mV` in the telemetry while the probe measures the floor |
+| `MOSS_ONE_TRACK_PIVOT` | a pivot whose one track freezes while the other turns is not a contact: that track did not turn in that direction under the pivot's load. It is braked (0) and the pivot turns on the other track for the rest of the run, said once in the log; both tracks frozen in that turn is a contact. On by default, 0 = such a pivot is a stall | the stack log: `pivot ... did not turn` |
Until `MOSS_TICKS_PER_M` is set, `read_odometry()` returns `None` and
`read_velocities()` returns the commanded twist: an honest "no measurement"
rather than a pose made of guesses. Set it and both come from the encoders.
The other base knobs: `MOSS_MAX_PCT` (percent ceiling, 100 by default, never
-above the firmware's 100), `MOSS_PULSE_MS` (full-duty pulse period, 500 by default, 0 = off), `MOSS_MIN_PCT` (static-friction floor when pulsing is off, 100 by default),
+above the firmware's 100), `MOSS_PULSE_MS` (full-duty pulse period, 0 = off, the default: continuous power), `MOSS_MIN_PCT` (static-friction floor when pulsing is off, 50 by default), `MOSS_PIVOT_PCT` (every pivot sent at this percent on both tracks, 100 by default),
`MOSS_RAMP_MS` (`"UP DOWN"`, sent as `ramp UP DOWN` at connect), `MOSS_PORT`
(the serial port; by default the QT Py's `/dev/serial/by-id` path; only a
`dimos run` by hand reads it from the shell, `tools/moss-teleop.sh` and
diff --git a/docs/cockpit.md b/docs/cockpit.md
index 2aebf58..fcfc1da 100644
--- a/docs/cockpit.md
+++ b/docs/cockpit.md
@@ -156,28 +156,31 @@ everywhere. The values are PLACEHOLDERS until measured on the robot:
| track centre distance | 0.266 m (CAD) | `MOSS_TRACK_M` |
| nose camera position on the base | 0.156, 0, 0.075 m (CAD) | `MOSS_CAMERA_XYZ_M="0.156 0 0.075"` |
| nose camera pitch | 0 deg | `MOSS_CAMERA_PITCH_DEG` |
-| full-duty pulse period of the base adapter (ms, 0 = off) | 500 | `MOSS_PULSE_MS` |
-| static-friction floor of the base adapter when pulsing is off (percent) | 100 | `MOSS_MIN_PCT` |
+| full-duty pulse period of the base adapter (ms, 0 = off) | 0 | `MOSS_PULSE_MS` |
+| static-friction floor of the base adapter when pulsing is off (percent) | 50 | `MOSS_MIN_PCT` |
+| percent every pivot is sent at, both tracks (0 = as asked) | 100 | `MOSS_PIVOT_PCT` |
+| bus voltage the floor was measured at, for the battery compensation (mV, 0 = off) | 12400 | `MOSS_BUS_REF_MV` |
+| a pivot with one frozen track turns on the other track instead of counting as a contact | on | `MOSS_ONE_TRACK_PIVOT` |
The speed knobs are `MOSS_MAX_PCT` (the PWM cap of the base adapter, 100 by
default) and `MOSS_NERF_SPEED` (the launcher's `--nerf-speed` for the planner,
-0.5 by default).
-
-The tracks of MOSS only turn at full duty on the floor (the geared motors
-behind the MDD3A), so the base adapter delivers every non-zero command as
-full-duty PULSES: a percent becomes a duty over a period of `MOSS_PULSE_MS`
-(500 by default, five slices of 100 ms): 40 % = two slices at full power then
-three off, 100 % = always on, any non-zero command = at least one slice. The
-planner's slowest commands move the rover, and a turn is still a turn. The
-firmware ramp follows: while pulsing the launcher defaults `MOSS_RAMP_MS` to
-`1 1` (a slice reaches full duty in 100 ms); a slower fall (`"1 5"`) releases
-instead of braking between slices, a longer period (`1000`) makes one cycle per
-second. `tools/moss-pulse-test.sh sec|moyen|doux` drives three bursts with a
-preset so the profile can be seen on the floor, and prints the two variables to
-put on the launch line. `MOSS_PULSE_MS=0` turns pulsing off; then
-`MOSS_MIN_PCT` (100 by default) raises every non-zero command to the floor
-instead, and `tools/moss_floor_probe.py --go` measures a floor for a base whose
-tracks turn below full duty.
+0.7 by default).
+
+The base runs on continuous power: the tracks turn from 40 % on a full pack,
+so `MOSS_MIN_PCT` (50 by default, 10 of margin for the sag under load) raises
+every non-zero command to the floor, an arc scaled up whole so its curvature
+survives, and every pivot goes out at `MOSS_PIVOT_PCT` (100) on both tracks.
+A percent is a duty of the bus voltage, so a pack at 10.5 V under load puts
+15 % less on the motors than the 12.4 V the floor was measured at: the adapter
+scales every command by `MOSS_BUS_REF_MV` / bus (never below 1, at most 1.5,
+capped at 100). A pivot whose one track freezes while the other turns is that
+track not turning in that direction under the pivot's load, not a contact:
+the adapter brakes it and turns on the other track for the rest of the run
+(`MOSS_ONE_TRACK_PIVOT`, said once in the log). `tools/moss_floor_probe.py
+--go` measures the floor (raw percents: it turns the compensation and the
+rewrite off), `tools/moss-pulse-test.sh` and `MOSS_PULSE_MS` keep the
+full-duty pulse mode for a base whose tracks only turn at full power
+([base.md](base.md)).
The lidar's floor projection is `base_link`, the rover's rotation centre: mount
the C1 there, or the footprint and the pivot clearance are off by its offset.
diff --git a/moss_dimos/moss_adapter.py b/moss_dimos/moss_adapter.py
index f2c0221..cdaa072 100644
--- a/moss_dimos/moss_adapter.py
+++ b/moss_dimos/moss_adapter.py
@@ -60,7 +60,9 @@ Calibration (env, all optional):
(default robot_profile.MOSS.track_m, 0.266 CAD, placeholder)
MOSS_MPS_AT_LIMIT floor speed in m/s at MOSS_MAX_PCT, measured
MOSS_MAX_PCT percent ceiling, <= the firmware's 100
- MOSS_MIN_PCT static-friction floor in percent (0 = off, default 50: measured 40 on a full battery, 10 of margin for the sag): a non-zero track command below it is raised to it, sign kept
+ MOSS_MIN_PCT static-friction floor in percent (0 = off, default 50: measured 40 on a full battery, 10 of margin for the sag): a non-zero track command below it is raised to it, sign kept; an arc is scaled up whole so its curvature survives (floor_tracks)
+ MOSS_BUS_REF_MV the bus voltage the floor and the speeds were measured at (default 12400): every command is scaled by ref/bus so a sagging pack puts the same voltage on the motors (never below 1, at most x1.5, capped at the limit); 0 = off
+ MOSS_ONE_TRACK_PIVOT a pivot whose one track freezes while the other turns is not a contact: that track is braked and the pivot turns on the other, for the rest of the run (default on; 0 = off, such a pivot is a stall)
MOSS_TICKS_PER_M encoder ticks per metre of track travel, measured
MOSS_TURN_GAIN turn authority vs forward speed (teleop_speeds), 0.5..4, default 2
MOSS_RAMP_MS "UP DOWN" sent as 'ramp UP DOWN' after connect (while pulsing, unset = "1 1")
@@ -117,6 +119,14 @@ PULSE_RAMP = "1 1" # firmware ramp while pulsing: 1 ms per %, so a 1
PIVOT_PCT_DEFAULT = 100 # MOSS_PIVOT_PCT: a pivot (tracks in opposite directions) is sent at this percent on both
# tracks whatever the planner asked, 0 = off. The pad pivots at 100 % continuous and the
# tracks turn without jolts there; the planner's slow pivots (8 %) can only be jolts
+BUS_REF_MV_DEFAULT = 12400 # MOSS_BUS_REF_MV: the bus voltage the floor and the speeds were measured at (a full 3S pack).
+ # A percent is a duty of the bus: at 10.5 V under load the same 50 % puts 15 % less voltage on
+ # the motor and the tracks creep or freeze, so every command is scaled by ref/bus (capped at
+ # BUS_GAIN_MAX, never below 1, never above the limit). 0 = off.
+BUS_GAIN_MAX = 1.5 # the scale never goes past this (a 3S pack at 8.3 V is a pack to stop on, not to compensate)
+BUS_SANE_MV = (6000, 20000) # a bus reading outside this is a broken sensor: no compensation
+ONE_TRACK_PIVOT_DEFAULT = True # MOSS_ONE_TRACK_PIVOT: a pivot whose one track does not turn (the other does) is not a contact:
+ # the frozen track is braked (0) and the pivot turns on the other track, for the rest of the run
PULSE_MIN_PCT_DEFAULT = 40 # MOSS_PULSE_MIN_PCT: the lowest duty of a non-zero track while pulsing; a planner pivot at
# 8 % would be one 100 ms slice per second (seen on the floor: jerky and slow), 40 % moves the rover
@@ -136,6 +146,68 @@ def pivot_pct() -> int:
return max(0, min(100, int(_env_float("MOSS_PIVOT_PCT", PIVOT_PCT_DEFAULT))))
+def bus_ref_mv() -> int:
+ """MOSS_BUS_REF_MV clamped to 0..20000 (0 = no compensation)."""
+ return max(0, min(20000, int(_env_float("MOSS_BUS_REF_MV", BUS_REF_MV_DEFAULT))))
+
+
+def bus_gain(bus_mv, ref_mv: int) -> float:
+ """The scale that puts the reference voltage back on the motors: ref/bus,
+ never below 1 (a full pack is never slowed down), never above BUS_GAIN_MAX;
+ 1 without a reference, without a reading, or with a reading outside BUS_SANE_MV."""
+ try:
+ bus = int(bus_mv)
+ except (TypeError, ValueError):
+ return 1.0
+ if ref_mv <= 0 or not BUS_SANE_MV[0] <= bus <= BUS_SANE_MV[1]:
+ return 1.0
+ return max(1.0, min(BUS_GAIN_MAX, ref_mv / bus))
+
+
+def bus_scaled(want: tuple[int, int], gain: float, lim: int) -> tuple[int, int]:
+ """Each track scaled by the gain, sign kept, capped at the limit; a 0 stays 0."""
+ def one(p: int) -> int:
+ if p == 0 or gain == 1.0:
+ return p
+ return int(min(lim, round(abs(p) * gain))) * (1 if p > 0 else -1)
+ return one(want[0]), one(want[1])
+
+
+def one_track_pivot_on() -> bool:
+ """MOSS_ONE_TRACK_PIVOT: on unless 0 / false / no / off."""
+ return robot_profile.env_on("MOSS_ONE_TRACK_PIVOT", ONE_TRACK_PIVOT_DEFAULT)
+
+
+def floor_tracks(want: tuple[int, int], floor: int, lim: int) -> tuple[int, int]:
+ """The static-friction floor, the curvature kept. A non-zero track below
+ the floor does not turn, so a lone track (the other at 0) and a pivot
+ (opposite signs) are raised to the floor, sign kept. An arc (same signs)
+ is scaled up until its inner track reaches the floor, so the ratio the
+ commander asked for survives: raising only the inner track straightens
+ the arc (57/32 asked, 56/50 sent: the rover goes nearly straight and the
+ planner insists on the turn for twenty seconds). When the outer track
+ would pass the limit it is capped there, and the inner track, below the
+ floor again, goes to 0: a one-track turn, tighter than asked but in the
+ asked direction, which the planner corrects at its next tick."""
+ left, right = want
+ if floor <= 0 or (left == 0 and right == 0):
+ return want
+ if left == 0 or right == 0 or (left > 0) != (right > 0):
+ def raise_(p: int) -> int:
+ return p if p == 0 or abs(p) >= floor else (floor if p > 0 else -floor)
+ return raise_(left), raise_(right)
+ inner, outer = min(abs(left), abs(right)), max(abs(left), abs(right))
+ if inner >= floor:
+ return want
+ sign = 1 if left > 0 else -1
+ k = floor / inner
+ if outer * k > lim + 0.5:
+ inner_capped = int(round(lim * inner / outer))
+ inner_out = inner_capped if inner_capped >= floor else 0
+ return (sign * lim, sign * inner_out) if abs(left) >= abs(right) else (sign * inner_out, sign * lim)
+ return sign * int(round(abs(left) * k)), sign * int(round(abs(right) * k))
+
+
def pivot_tracks(want: tuple[int, int], pct: int) -> tuple[int, int]:
"""A pivot (left and right non-zero, opposite signs) is sent at +-pct on both
tracks, the sign of each kept; anything else passes unchanged."""
@@ -236,8 +308,9 @@ def twist_to_tracks(vx: float, wz: float, g: MossGeometry) -> tuple[int, int]:
Differential drive: v_left = vx - wz*track/2, v_right = vx + wz*track/2.
Percent is proportional to speed with g.mps_at_limit <-> g.max_pct.
A twist too small to round to 1 % IS a stop (0, 0). A non-zero percent
- below g.min_pct is raised to g.min_pct with its sign (static-friction
- floor: the tracks do not turn below it, so the alternative is a stall)."""
+ below g.min_pct is raised by floor_tracks (static-friction floor: the
+ tracks do not turn below it, so the alternative is a stall), which keeps
+ the curvature of an arc instead of straightening it."""
half = g.track_m / 2.0
v_l = vx - wz * half
v_r = vx + wz * half
@@ -246,13 +319,9 @@ def twist_to_tracks(vx: float, wz: float, g: MossGeometry) -> tuple[int, int]:
floor = max(0, min(lim, int(g.min_pct)))
def pct(v: float) -> int:
- p = int(round(v * scale))
- p = max(-lim, min(lim, p))
- if p and abs(p) < floor:
- p = floor if p > 0 else -floor
- return p
+ return max(-lim, min(lim, int(round(v * scale))))
- return pct(v_l), pct(v_r)
+ return floor_tracks((pct(v_l), pct(v_r)), floor, lim)
TURN_GAIN_DEFAULT = 2.0 # MOSS_TURN_GAIN: turn authority vs forward speed, see teleop_speeds
@@ -342,6 +411,9 @@ class MossBaseAdapter:
self._stall_count = 0 # stalls since connect (telemetry_payload, the coordinator's bump)
self._pushing: tuple[int, int] | None = None # the command being pushed harder before a contact is declared
self._push_origin: tuple[int, int] | None = None # the commander's own pair while the push runs (its repeats keep the push)
+ self._bus_ref_mv = bus_ref_mv() # the voltage the percents were measured at (0 = no compensation)
+ self._one_track = one_track_pivot_on() # a pivot with one frozen track turns on the other track
+ self._dead_pivots: set[tuple[str, int]] = set() # (side, sign) that did not turn in a pivot: braked in such pivots from then on
# the mapping geometry: when pulsing, the static-friction floor becomes the duty floor of the pulses
self._map_geometry = replace(self._geometry, min_pct=pulse_min_pct()) if self._pulse_n else self._geometry
self._last_logged = None
@@ -577,7 +649,7 @@ class MossBaseAdapter:
vx, vy, wz = float(vx), float(vy), float(wz)
except Exception:
return False
- want = pivot_tracks(twist_to_tracks(vx, wz, self._map_geometry), self._pivot_pct)
+ want = self._pivot_fallback(pivot_tracks(twist_to_tracks(vx, wz, self._map_geometry), self._pivot_pct))
if want != (0, 0) and not self._enabled:
return False
if self._pushing is not None:
@@ -585,7 +657,7 @@ class MossBaseAdapter:
want = self._pushing # the commander repeats itself: the push goes on, and is what is sent
else:
self._pushing = self._push_origin = None # a new order: the push is over
- left, right = self._slice(want)
+ left, right = self._to_wire(want)
ok = self._drive(left, right)
if ok:
if want == (0, 0):
@@ -608,6 +680,30 @@ class MossBaseAdapter:
return want
return pulse_tracks(want, self._tick, self._pulse_n, int(self._geometry.max_pct))
+ def _gain(self) -> float:
+ """The bus compensation of the moment, from the last telemetry line."""
+ return bus_gain((self._telemetry or {}).get("bus_mV"), self._bus_ref_mv)
+
+ def _to_wire(self, want: tuple[int, int]) -> tuple[int, int]:
+ """What goes on the wire for the commander's pair: the bus compensation,
+ then the pulse slice of this tick when pulsing. The commander's pair
+ itself (`_want`) stays as asked: the stall guard and the push compare
+ percents between themselves, which the gain does not change."""
+ return self._slice(bus_scaled(want, self._gain(), int(self._geometry.max_pct)))
+
+ def _pivot_fallback(self, want: tuple[int, int]) -> tuple[int, int]:
+ """A pivot whose one track is known not to turn in that direction (it froze
+ while the other turned, see _stall_check) turns on the other track: the
+ dead side is sent 0 (braked). Anything else passes unchanged."""
+ left, right = want
+ if not self._dead_pivots or left == 0 or right == 0 or (left > 0) == (right > 0):
+ return want
+ if ("L", 1 if left > 0 else -1) in self._dead_pivots:
+ return 0, right
+ if ("R", 1 if right > 0 else -1) in self._dead_pivots:
+ return left, 0
+ return want
+
def _drive(self, left: int, right: int) -> bool:
"""Send 'drive L R'; on 'ERR not_armed' (arm expiry, invalid line,
the firmware's own deadman) re-arm and retry ONCE. Never pre-arm
@@ -668,7 +764,7 @@ class MossBaseAdapter:
continue
# pulsing: the slice of this tick, 'drive 0 0' included (an OFF slice
# keeps the firmware armed and its 300 ms deadman fed)
- self._target = self._slice(self._want)
+ self._target = self._to_wire(self._want)
self._send(f"drive {self._target[0]} {self._target[1]}")
# what the firmware actually applies, and the battery
# voltage under load: one line per 2 s
@@ -713,6 +809,29 @@ class MossBaseAdapter:
if not (stalled_l or stalled_r):
self._pushing = self._push_origin = None
return False
+ # A pivot (opposite signs) with ONE frozen track while the other turns is not a
+ # contact: a track that does not turn in that direction under the pivot's load (the
+ # right track in reverse on a sagging pack, every pivot to the right became a stall,
+ # a back-off and an abandoned goal). From now on such a pivot turns on the other
+ # track, the dead one braked at 0; both tracks frozen in that turn is a contact.
+ pivot = want_l != 0 and want_r != 0 and (want_l > 0) != (want_r > 0)
+ if pivot and stalled_l != stalled_r and self._one_track:
+ side = "R" if stalled_r else "L"
+ sign = 1 if (want_r if stalled_r else want_l) > 0 else -1
+ if (side, sign) not in self._dead_pivots:
+ self._dead_pivots.add((side, sign))
+ _log().warning(f"MOSS base: pivot L={want_l:+d}% R={want_r:+d}%: the {'right' if stalled_r else 'left'} track "
+ f"did not turn (encoders L={l_new - l_old:+d} R={r_new - r_old:+d} ticks in {t_new - t_old:.1f} s) "
+ f"- not a contact: from now on this pivot turns on the {'left' if stalled_r else 'right'} track "
+ f"only, the {'right' if stalled_r else 'left'} track braked")
+ new_want = (want_l, 0) if stalled_r else (0, want_r)
+ self._pushing = self._push_origin = None
+ self._want = new_want
+ self._drive_since = now
+ self._tick_hist = self._tick_hist[-1:]
+ self._target = self._to_wire(new_want)
+ self._send(f"drive {self._target[0]} {self._target[1]}")
+ return True
# Both commanded tracks frozen = the rover pushes against something. One track frozen
# while the other turns is a contact only when the frozen track was commanded at least
# as hard as the moving one (it should have turned at least as much: something holds
@@ -766,8 +885,11 @@ class MossBaseAdapter:
self._last_logged = pct
head = "MOSS base MOCK:" if self._mock else "MOSS base:"
vx, vy, wz = twist
+ gain = self._gain()
+ wire = bus_scaled(pct, gain, int(self._geometry.max_pct))
+ comp = "" if gain == 1.0 else f" x{gain:.2f} for the bus at {int((self._telemetry or {}).get('bus_mV') or 0) / 1000:.1f} V = L={wire[0]:+d}% R={wire[1]:+d}%"
_log().info(f"{head} twist vx={vx:+.3f} vy={vy:+.3f}(ignored) wz={wz:+.3f} "
- f"-> tracks L={pct[0]:+d}% R={pct[1]:+d}% (limit {self._geometry.max_pct}%)")
+ f"-> tracks L={pct[0]:+d}% R={pct[1]:+d}%{comp} (limit {self._geometry.max_pct}%)")
# ── feedback ───────────────────────────────────────────────────────
def _ingest_ticks(self, tel: dict) -> None:
diff --git a/tools/moss_floor_probe.py b/tools/moss_floor_probe.py
index 42c5978..b81b731 100755
--- a/tools/moss_floor_probe.py
+++ b/tools/moss_floor_probe.py
@@ -152,6 +152,8 @@ def main(argv=None) -> int:
print("a dimOS stack is running: stop it first (tools/moss-fly.sh stop), this tool opens the motor board itself")
return 2
os.environ["MOSS_PIVOT_PCT"] = "0" # the percent printed is the percent sent, pivots included
+ os.environ["MOSS_BUS_REF_MV"] = "0" # ...and not scaled for the battery: this tool measures the raw percent
+ os.environ["MOSS_ONE_TRACK_PIVOT"] = "0" # a pivot with one frozen track is reported as such, never rewritten
kw = {"geometry": MossGeometry(mps_at_limit=1.0, max_pct=100, min_pct=0)}
if a.mock:
os.environ["MOSS_MOCK_BUS"] = "1"