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.pyindex 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] = vreturn 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()