220 lines
8.4 KiB
Python
220 lines
8.4 KiB
Python
#!/usr/bin/env python3
|
|
"""Watch one axis's drive frame while you start homing from elsewhere.
|
|
|
|
Homing failures are hard to read after the fact: the drive reports a state, the
|
|
controller reports another, and by the time anyone looks, the motion has stopped
|
|
and the only evidence is "it ended up in the wrong place". This samples the
|
|
whole per-axis frame — commanded and measured position, the drive status word
|
|
(including the limit/home switch bits and the homing bits), the controller's
|
|
own motion and homing state, and the three mapped inputs — and prints a line
|
|
only when something changes.
|
|
|
|
Run it, then press 启动回零 in the app (or send the homing command from ZDevelop)
|
|
while it is sampling:
|
|
|
|
python apps\\autocal\\tools\\home_trace.py --axis 3 --seconds 45
|
|
python apps\\autocal\\tools\\home_trace.py --axis 3 --seconds 45 --csv trace.csv
|
|
|
|
Reading is non-mutating: it never enables, moves, homes or stops an axis. It
|
|
does open its own connection to the controller, so run it *instead of* having
|
|
the app open if two clients on one controller is a problem on your bench; the
|
|
ZMotion runtime generally allows it, but the app's own poller is the only writer
|
|
you want during a homing run.
|
|
|
|
What the lines mean (CiA-402 status word, exposed here as ``DRIVE_STATUS``):
|
|
|
|
====== ==================================================
|
|
bit 1 positive limit switch active
|
|
bit 2 negative limit switch active
|
|
bit 3 drive fault
|
|
bit 4 home (datum) switch active
|
|
bit 10 target reached
|
|
bit 12 homing attained — the drive *thinks* it found its reference
|
|
bit 13 homing error
|
|
====== ==================================================
|
|
|
|
A homing run that ends with bit 12 set and no error, at a position you did not
|
|
expect, means the drive accepted a reference the mechanical setup does not
|
|
agree with — usually a switch polarity the drive reads as "already made", not a
|
|
missing index pulse (which instead creeps until something stops it).
|
|
|
|
Exit status is 0 whether or not a run was seen: this is an observation tool, and
|
|
"nothing happened" is a result.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import csv
|
|
import ctypes
|
|
import os
|
|
import sys
|
|
import time
|
|
from pathlib import Path
|
|
|
|
APP_ROOT = Path(__file__).resolve().parents[1]
|
|
for path in (APP_ROOT / "src", APP_ROOT.parents[1] / "shared" / "src"):
|
|
if str(path) not in sys.path:
|
|
sys.path.insert(0, str(path))
|
|
|
|
os.environ.setdefault("ORISYS_APP_ID", "autocal")
|
|
|
|
#: Fields sampled per tick, in the order the header and CSV are written.
|
|
FIELDS = (
|
|
"t",
|
|
"dpos",
|
|
"mpos",
|
|
"idle",
|
|
"drive_status",
|
|
"p_ot",
|
|
"n_ot",
|
|
"fault",
|
|
"home_sw",
|
|
"target_reached",
|
|
"homing_attained",
|
|
"homing_error",
|
|
"axis_status",
|
|
"home_status",
|
|
"in_rev",
|
|
"in_fwd",
|
|
"in_datum",
|
|
)
|
|
|
|
#: Position change above this counts as motion, in user units.
|
|
MOVE_EPSILON = 1e-6
|
|
|
|
|
|
def _bit(value: int, index: int) -> int:
|
|
return (int(value) >> index) & 1
|
|
|
|
|
|
class AxisReader:
|
|
"""The few controller calls this tool needs, on one connection."""
|
|
|
|
def __init__(self, ip: str) -> None:
|
|
from orisys_app_shared.zmotion import ZmotionClient
|
|
|
|
self._client = ZmotionClient().load()
|
|
result = self._client.open_eth(ip)
|
|
result.raise_for_status(f"open {ip}")
|
|
self._handle = self._client._require_handle() # noqa: SLF001 - one client
|
|
|
|
def close(self) -> None:
|
|
self._client.close()
|
|
|
|
def _param(self, name: str, axis: int) -> float | None:
|
|
value = ctypes.c_float()
|
|
code = self._client._dll.ZAux_Direct_GetParam( # noqa: SLF001
|
|
self._handle, name.encode(), ctypes.c_int(axis), ctypes.byref(value)
|
|
)
|
|
return float(value.value) if code == 0 else None
|
|
|
|
def _input(self, pin: int) -> int | None:
|
|
value = ctypes.c_int()
|
|
code = self._client._dll.ZAux_Direct_GetIn( # noqa: SLF001
|
|
self._handle, ctypes.c_int(pin), ctypes.byref(value)
|
|
)
|
|
return int(value.value) if code == 0 else None
|
|
|
|
def sample(self, axis: int, elapsed: float) -> dict:
|
|
raw_status = self._param("DRIVE_STATUS", axis)
|
|
status = int(raw_status or 0) & 0xFFFF
|
|
# The three switch inputs are mapped contiguously by ECAT.bas / RTEX.bas:
|
|
# REV at 128 + 16*axis, then FWD and DATUM.
|
|
base = 128 + 16 * axis
|
|
return {
|
|
"t": round(elapsed, 3),
|
|
"dpos": self._param("DPOS", axis),
|
|
"mpos": self._param("MPOS", axis),
|
|
"idle": self._param("IDLE", axis),
|
|
"drive_status": f"0x{status:04x}",
|
|
"p_ot": _bit(status, 1),
|
|
"n_ot": _bit(status, 2),
|
|
"fault": _bit(status, 3),
|
|
"home_sw": _bit(status, 4),
|
|
"target_reached": _bit(status, 10),
|
|
"homing_attained": _bit(status, 12),
|
|
"homing_error": _bit(status, 13),
|
|
"axis_status": self._client.get_axis_status(axis).value_or_none,
|
|
"home_status": self._client.bus_home_status(axis).value_or_none,
|
|
"in_rev": self._input(base),
|
|
"in_fwd": self._input(base + 1),
|
|
"in_datum": self._input(base + 2),
|
|
}
|
|
|
|
|
|
def _render(row: dict) -> str:
|
|
return (
|
|
f"t={row['t']:7.3f} {row['drive_status']} "
|
|
f"OT+={row['p_ot']} OT-={row['n_ot']} HOME={row['home_sw']} FAULT={row['fault']} "
|
|
f"REACHED={row['target_reached']} HOMED={row['homing_attained']} "
|
|
f"HOMERR={row['homing_error']} "
|
|
f"DPOS={row['dpos']} IDLE={row['idle']} ctrl_home_status={row['home_status']} "
|
|
f"IN={row['in_rev']}/{row['in_fwd']}/{row['in_datum']}"
|
|
)
|
|
|
|
|
|
def main(argv: list[str] | None = None) -> int:
|
|
parser = argparse.ArgumentParser(prog="home-trace", description=__doc__)
|
|
parser.add_argument("--axis", type=int, required=True, help="axis number to watch")
|
|
parser.add_argument("--seconds", type=float, default=30.0, help="how long to sample")
|
|
parser.add_argument("--ip", default="192.168.0.11", help="controller address")
|
|
parser.add_argument("--interval-ms", type=float, default=20.0, help="sample period")
|
|
parser.add_argument("--csv", default="", help="also write every sample to this CSV")
|
|
args = parser.parse_args(argv)
|
|
|
|
reader = AxisReader(args.ip)
|
|
print(f"watching axis {args.axis} for {args.seconds:g}s — start homing now", flush=True)
|
|
period = max(5.0, args.interval_ms) / 1000.0
|
|
deadline = time.monotonic() + args.seconds
|
|
started = time.monotonic()
|
|
|
|
rows: list[dict] = []
|
|
previous: dict | None = None
|
|
movements = 0
|
|
try:
|
|
while time.monotonic() < deadline:
|
|
row = reader.sample(args.axis, time.monotonic() - started)
|
|
rows.append(row)
|
|
interesting = ("drive_status", "idle", "p_ot", "n_ot", "home_sw", "fault",
|
|
"target_reached", "homing_attained", "homing_error",
|
|
"axis_status", "home_status", "in_rev", "in_fwd", "in_datum")
|
|
changed = previous is None or any(row[k] != previous[k] for k in interesting)
|
|
moved = previous is not None and row["mpos"] is not None and previous["mpos"] is not None \
|
|
and abs(row["mpos"] - previous["mpos"]) > MOVE_EPSILON
|
|
if moved and not changed:
|
|
movements += 1
|
|
if movements % 25 == 0: # a heartbeat while it travels
|
|
print(" " + _render(row), flush=True)
|
|
elif changed:
|
|
print(" " + _render(row), flush=True)
|
|
previous = row
|
|
time.sleep(period)
|
|
except KeyboardInterrupt: # pragma: no cover - operator convenience
|
|
print("\ninterrupted", flush=True)
|
|
finally:
|
|
reader.close()
|
|
|
|
if args.csv:
|
|
path = Path(args.csv)
|
|
with path.open("w", newline="", encoding="utf-8") as handle:
|
|
writer = csv.DictWriter(handle, fieldnames=list(FIELDS))
|
|
writer.writeheader()
|
|
writer.writerows(rows)
|
|
print(f"wrote {len(rows)} samples to {path}", flush=True)
|
|
|
|
if previous is not None:
|
|
print("final: " + _render(previous), flush=True)
|
|
if previous["homing_attained"] and not previous["homing_error"]:
|
|
print(
|
|
"note: the drive reports homing attained — if the position is not where the "
|
|
"mechanics say home is, the switch polarity the drive sees is wrong (it read "
|
|
"'already made'), or the wrong switch edge was chosen.",
|
|
flush=True,
|
|
)
|
|
return 0
|
|
|
|
|
|
if __name__ == "__main__":
|
|
raise SystemExit(main())
|