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

arm calibration: --sweep measures every joint's end stops (unwrapped on the encoder circle), centres each travel on 2048 and writes offsets and limits; reads retried, the gripper keeps its own register map

metrox3289468c5484
diff --git a/tools/moss_arm_calibrate.py b/tools/moss_arm_calibrate.py
index 52dbb13..0466d5b 100644
--- a/tools/moss_arm_calibrate.py
+++ b/tools/moss_arm_calibrate.py
@@ -1,16 +1,17 @@
#!/usr/bin/env python3
"""Calibrate the arm's Feetech servos (STS family, protocol 0, 1 Mbps), the SO-101 way.
-Two steps, each on the operator's word (the arm is moved by hand, torque off):
- 1. --middle the arm is held in the middle of every joint's travel: the servo's
- Homing_Offset is written so that this pose reads 2048 (the centre of
- the 0..4095 turn), which puts the encoder's zero crossing OUT of the
- useful range.
- 2. --range every joint has just been moved through its whole travel: the lowest
- and highest positions seen during --watch are written as the servo's
- Min/Max_Position_Limit and saved with the offsets.
- --watch N read positions for N seconds and print the min/max per joint (used by
- --range; run it while the operator sweeps the joints).
+One step, on the operator's word (the arm is moved by hand, torque off):
+ --sweep the operator moves every joint from one end stop to the other, then
+ stops the sweep (SIGINT / SIGTERM, or --watch seconds). Positions are
+ unwrapped on the 0..4095 circle, so a travel that crosses the encoder's
+ zero is measured right. For every joint seen: the middle of its travel
+ becomes 2048 (Homing_Offset, its sign checked against the servo), the
+ end stops become Min/Max_Position_Limit, and all of it is saved.
+ --middle older flow: the arm is held in the middle of every travel and the
+ offsets are written so that this pose reads 2048 (no limits).
+ --range older flow: write the raw min/max seen during --watch as the limits.
+ --watch N read positions for N seconds (used by --sweep and --range).
--show read what the servos hold now (offset, limits, position).
Registers (STS/SMS control table): Homing_Offset 31 (2 bytes, sign-magnitude, bit 11 =
@@ -53,11 +54,27 @@ def open_bus():
return scs, ph, scs.PacketHandler(0)
+def read2(scs, ph, pk, sid: int, addr: int, tries: int = 6):
+ """A two-byte read with retries: the SDK raises on a truncated answer (seen right
+ after an EEPROM write), and a bus collision gives a failed result. None when it never came."""
+ for _ in range(tries):
+ try:
+ v, comm, _err = pk.read2ByteTxRx(ph, sid, addr)
+ if comm == scs.COMM_SUCCESS:
+ return v
+ except Exception: # noqa: BLE001 - a short packet inside the SDK
+ pass
+ time.sleep(0.03)
+ return None
+
+
def present(scs, ph, pk):
out = {}
for name, sid in JOINTS.items():
if pk.ping(ph, sid)[1] == scs.COMM_SUCCESS:
- out[name] = pk.read2ByteTxRx(ph, sid, 56)[0]
+ v = read2(scs, ph, pk, sid, 56)
+ if v is not None:
+ out[name] = v
return out
@@ -88,12 +105,117 @@ def save(doc):
print(f"saved {OUT}")
+def unwrap_step(prev_raw: int, raw: int) -> int:
+ """The signed step between two raw readings on the 0..4095 circle (the short way round)."""
+ d = raw - prev_raw
+ if d > 2048:
+ d -= 4096
+ elif d < -2048:
+ d += 4096
+ return d
+
+
+def plan_from_sweep(lo_u: float, hi_u: float, raw_now: int, unwrapped_now: float, old_offset: int) -> dict:
+ """From an unwrapped travel [lo_u, hi_u] (in the frame the servo reports now, offset `old_offset`
+ included) the homing offset that puts the middle of the travel at 2048, and the limits in
+ that new frame. Pure, benched."""
+ centre_u = (lo_u + hi_u) / 2.0
+ # the reported reading of the centre, on the circle
+ centre_reported = (raw_now + (centre_u - unwrapped_now)) % 4096
+ # reported = raw - offset -> raw at the centre = reported + old
+ raw_centre = (centre_reported + old_offset) % 4096
+ off = int(round(raw_centre - CENTRE))
+ if off > 2047:
+ off -= 4096
+ if off < -2047:
+ off += 4096
+ half = (hi_u - lo_u) / 2.0
+ lo = int(round(max(0, CENTRE - half)))
+ hi = int(round(min(4095, CENTRE + half)))
+ return {"homing_offset": off, "range_min": lo, "range_max": hi, "travel": int(round(hi_u - lo_u))}
+
+
+def sweep(scs, ph, pk, seconds: float, doc: dict) -> int:
+ """Read every joint while the operator sweeps them by hand; on their word (SIGINT) or after
+ `seconds`, centre each travel on 2048 and write offsets and limits."""
+ import signal
+ pos0 = present(scs, ph, pk)
+ if not pos0:
+ print("no servo answers"); return 1
+ unwrapped = {n: float(p) for n, p in pos0.items()}
+ last_raw = dict(pos0)
+ lo = dict(unwrapped); hi = dict(unwrapped)
+ stop = {"now": False}
+ for sig in (signal.SIGINT, signal.SIGTERM):
+ signal.signal(sig, lambda *_: stop.__setitem__("now", True))
+ print("sweep: move every joint from one end stop to the other, then stop this tool (SIGINT)", flush=True)
+ t0 = time.monotonic(); last_print = 0.0
+ while not stop["now"] and time.monotonic() - t0 < seconds:
+ for name, raw in present(scs, ph, pk).items():
+ if name not in last_raw:
+ continue
+ unwrapped[name] += unwrap_step(last_raw[name], raw)
+ last_raw[name] = raw
+ lo[name] = min(lo[name], unwrapped[name]); hi[name] = max(hi[name], unwrapped[name])
+ if time.monotonic() - last_print > 2.0:
+ last_print = time.monotonic()
+ print("travel so far:", {n: int(hi[n] - lo[n]) for n in lo}, flush=True)
+ time.sleep(0.05)
+ print("travel seen:", {n: (int(lo[n]), int(hi[n]), int(hi[n] - lo[n])) for n in lo}, flush=True)
+ for name in lo:
+ sid = JOINTS[name]
+ if hi[name] - lo[name] < 50:
+ print(f" {name:14s} id {sid}: travel {int(hi[name] - lo[name])} ticks, not moved: skipped"); continue
+ if name == "gripper":
+ # the CF35-12's register map is its own: limits only, in the frame it reports
+ glo, ghi = int(max(0, lo[name])), int(min(4095, hi[name]))
+ ok1 = write_eeprom(scs, ph, pk, sid, 9, glo, 2)
+ ok2 = write_eeprom(scs, ph, pk, sid, 11, ghi, 2)
+ doc.setdefault(name, {})
+ doc[name].update({"id": sid, "range_min": glo, "range_max": ghi, "travel_ticks": int(hi[name] - lo[name])})
+ print(f" {name:14s} id {sid}: travel {int(hi[name] - lo[name])} ticks -> limits {glo}..{ghi} "
+ f"({'OK' if ok1 and ok2 else 'FAILED'}), no offset on this servo", flush=True)
+ continue
+ old = decode_sign_magnitude(read2(scs, ph, pk, sid, 31) or 0)
+ p_before = read2(scs, ph, pk, sid, 56)
+ plan = plan_from_sweep(lo[name], hi[name], last_raw[name], unwrapped[name], old)
+ off = plan["homing_offset"]
+ ok = write_eeprom(scs, ph, pk, sid, 31, encode_sign_magnitude(off), 2)
+ time.sleep(0.05)
+ p_after = read2(scs, ph, pk, sid, 56)
+ if p_before is None or p_after is None:
+ print(f" {name:14s} id {sid}: offset {off:+d} written but the servo did not answer the read-back: check with --show", flush=True)
+ p_before = p_after = 0
+ expect = (p_before - (off - old)) % 4096
+ if abs(unwrap_step(expect, p_after)) > 8:
+ # the servo applies the offset the other way round: negate and check again
+ ok = write_eeprom(scs, ph, pk, sid, 31, encode_sign_magnitude(-off), 2)
+ time.sleep(0.05)
+ p_after = read2(scs, ph, pk, sid, 56) or 0
+ if abs(unwrap_step(expect, p_after)) <= 8:
+ off = -off
+ else:
+ print(f" {name:14s} id {sid}: offset {off:+d} written but the reading moved from {p_before} to {p_after} "
+ f"(expected {expect}): check by hand", flush=True)
+ ok1 = write_eeprom(scs, ph, pk, sid, 9, plan["range_min"], 2)
+ ok2 = write_eeprom(scs, ph, pk, sid, 11, plan["range_max"], 2)
+ doc.setdefault(name, {})
+ doc[name].update({"id": sid, "homing_offset": off, "range_min": plan["range_min"], "range_max": plan["range_max"],
+ "travel_ticks": plan["travel"]})
+ print(f" {name:14s} id {sid}: travel {plan['travel']} ticks -> offset {off:+d} ({'OK' if ok else 'FAILED'}), "
+ f"limits {plan['range_min']}..{plan['range_max']} ({'OK' if ok1 and ok2 else 'FAILED'}), reads {p_after} now", flush=True)
+ save(doc)
+ ph.closePort()
+ return 0
+
+
def main(argv=None) -> int:
ap = argparse.ArgumentParser(description=__doc__.splitlines()[0])
ap.add_argument("--show", action="store_true")
ap.add_argument("--middle", action="store_true", help="write homing offsets so the current pose reads 2048")
ap.add_argument("--range", action="store_true", help="write the min/max seen during --watch as position limits")
ap.add_argument("--watch", type=float, default=0.0, help="seconds to read positions and track min/max")
+ ap.add_argument("--sweep", action="store_true", help="measure the end stops of every joint (unwrapped), centre them on 2048, write offsets and limits")
a = ap.parse_args(argv)
scs, ph, pk = open_bus()
pos = present(scs, ph, pk)
@@ -128,6 +250,8 @@ def main(argv=None) -> int:
doc[name].update({"id": sid, "homing_offset": off})
print(f" {name:14s} id {sid}: was {p} (offset {old:+d}) -> offset {off:+d} written {'OK' if okw else 'FAILED'}, now reads {after}")
save(doc)
+ if a.sweep:
+ return sweep(scs, ph, pk, a.watch or 900.0, doc)
if a.watch > 0:
lo = dict(present(scs, ph, pk)); hi = dict(lo)
t0 = time.monotonic()