188 lines
8.0 KiB
Python
188 lines
8.0 KiB
Python
#!/usr/bin/env python3
|
|
"""Does the switch signal actually toggle? Sample the bits, count the edges.
|
|
|
|
``home_trace.py`` prints a line whenever anything about the axis changes, which
|
|
is right for watching a homing run. This does the opposite job: it samples as
|
|
fast as the controller answers, counts every transition of the switch bits, and
|
|
reports them as a total. That turns "I think the switch works" into a number.
|
|
|
|
Use it to answer one question — *does this switch reach the drive at all?* — for
|
|
a rotary axis where a partial move may simply not cross the switch:
|
|
|
|
# drive the axis through at least one full revolution while this runs
|
|
python apps\\autocal\\tools\\switch_probe.py --axis 3 --seconds 60
|
|
python apps\\autocal\\tools\\switch_probe.py --axis 3 --seconds 60 --csv edges3.csv
|
|
|
|
Interpretation:
|
|
|
|
* **``HOME`` edges > 0** — the bit follows the switch. Its polarity is the
|
|
``rest→active`` pair printed in the summary; pick the homing method whose
|
|
edge matches.
|
|
* **``HOME`` edges = 0 while ``travel`` covers a full revolution** — the signal
|
|
never reached the drive. On a fail-safe input wired normally closed, a broken
|
|
wire looks exactly like "switch made" (the drive reads one constant level), so
|
|
zero edges after a full turn is a *wiring* finding, not a homing-method one.
|
|
Change methods all you like; no CiA-402 method can use a signal the drive
|
|
does not see.
|
|
|
|
Reading is non-mutating: it never enables, moves, homes or stops anything. You
|
|
drive the axis (app jog, MPG, or hand), this only listens.
|
|
|
|
Exit status is 0 in all outcomes — including "no edges", which 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")
|
|
|
|
#: The bits worth counting, as ``label -> (bit index in DRIVE_STATUS, description)``.
|
|
WATCHED_BITS = {
|
|
"P_OT": (1, "positive limit switch"),
|
|
"N_OT": (2, "negative limit switch"),
|
|
"FAULT": (3, "drive fault"),
|
|
"HOME": (4, "home / datum switch"),
|
|
}
|
|
|
|
#: Reported alongside the drive bits, because the controller inverts these and
|
|
#: the pair is what tells a wiring fault from a working switch.
|
|
INPUT_OFFSETS = {"IN_REV": 0, "IN_FWD": 1, "IN_DATUM": 2} # from 128 + 16*axis
|
|
|
|
|
|
class Sampler:
|
|
def __init__(self, ip: str) -> None:
|
|
from orisys_app_shared.zmotion import ZmotionClient
|
|
|
|
self._client = ZmotionClient().load()
|
|
self._client.open_eth(ip).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 now(self, axis: int) -> tuple[int, float | None, dict[str, int | None]]:
|
|
status = int(self.param("DRIVE_STATUS", axis) or 0) & 0xFFFF
|
|
base = 128 + 16 * axis
|
|
inputs = {name: self.input(base + offset) for name, offset in INPUT_OFFSETS.items()}
|
|
return status, self.param("MPOS", axis), inputs
|
|
|
|
|
|
def main(argv: list[str] | None = None) -> int:
|
|
parser = argparse.ArgumentParser(prog="switch-probe", description=__doc__)
|
|
parser.add_argument("--axis", type=int, required=True)
|
|
parser.add_argument("--seconds", type=float, default=60.0)
|
|
parser.add_argument("--ip", default="192.168.0.11")
|
|
parser.add_argument("--csv", default="", help="write the edge log to this CSV")
|
|
args = parser.parse_args(argv)
|
|
|
|
sampler = Sampler(args.ip)
|
|
print(
|
|
f"counting edges on axis {args.axis} for {args.seconds:g}s — "
|
|
f"drive the axis through a full revolution now",
|
|
flush=True,
|
|
)
|
|
|
|
deadline = time.monotonic() + args.seconds
|
|
edges: dict[str, list[tuple[float, int, int]]] = {name: [] for name in WATCHED_BITS}
|
|
input_edges: dict[str, list[tuple[float, int, int]]] = {name: [] for name in INPUT_OFFSETS}
|
|
counts: dict[str, int] = {name: 0 for name in WATCHED_BITS}
|
|
input_counts: dict[str, int] = {name: 0 for name in INPUT_OFFSETS}
|
|
previous: dict | None = None
|
|
travel = 0.0
|
|
samples = 0
|
|
events: list[dict] = []
|
|
start = time.monotonic()
|
|
|
|
def record(elapsed: float, status: int) -> None:
|
|
for label, (bit, _desc) in WATCHED_BITS.items():
|
|
value = (status >> bit) & 1
|
|
if previous is not None and value != ((previous["status"] >> bit) & 1):
|
|
counts[label] += 1
|
|
edges[label].append((elapsed, (previous["status"] >> bit) & 1, value))
|
|
events.append({"t": round(elapsed, 3), "kind": "drive_bit", "name": label,
|
|
"from": (previous["status"] >> bit) & 1, "to": value})
|
|
|
|
try:
|
|
while time.monotonic() < deadline:
|
|
status, mpos, inputs = sampler.now(args.axis)
|
|
elapsed = time.monotonic() - start
|
|
samples += 1
|
|
if previous is not None and mpos is not None and previous["mpos"] is not None:
|
|
travel += abs(mpos - previous["mpos"])
|
|
record(elapsed, status)
|
|
for label, value in inputs.items():
|
|
if previous is not None and value != previous["inputs"][label]:
|
|
input_counts[label] += 1
|
|
input_edges[label].append((elapsed, previous["inputs"][label], value))
|
|
events.append({"t": round(elapsed, 3), "kind": "input", "name": label,
|
|
"from": previous["inputs"][label], "to": value})
|
|
previous = {"status": status, "mpos": mpos, "inputs": inputs, "t": elapsed}
|
|
time.sleep(0.005)
|
|
except KeyboardInterrupt: # pragma: no cover - operator convenience
|
|
print("\ninterrupted", flush=True)
|
|
finally:
|
|
sampler.close()
|
|
|
|
first_status = None
|
|
if samples:
|
|
first_status = previous["status"]
|
|
print()
|
|
print(f"samples: {samples} total travel: {travel:.1f} user units")
|
|
print("drive bit edges:")
|
|
for label, (bit, desc) in WATCHED_BITS.items():
|
|
if counts[label]:
|
|
seq = " ".join(f"{a}->{b}" for _t, a, b in edges[label][:8])
|
|
print(f" {label:6s} ({desc}): {counts[label]} edges first: {seq}")
|
|
else:
|
|
print(f" {label:6s} ({desc}): 0 edges — constant at {(previous['status'] >> bit) & 1 if samples else '?'}")
|
|
print("controller input edges (INVERT_IN applied):")
|
|
for label in INPUT_OFFSETS:
|
|
state = previous["inputs"][label] if samples else "?"
|
|
print(f" {label:8s}: {input_counts[label]} edges constant at {state}")
|
|
|
|
if args.csv:
|
|
path = Path(args.csv)
|
|
with path.open("w", newline="", encoding="utf-8") as handle:
|
|
writer = csv.DictWriter(handle, fieldnames=["t", "kind", "name", "from", "to"])
|
|
writer.writeheader()
|
|
writer.writerows(events)
|
|
print(f"wrote {len(events)} edge events to {path}")
|
|
|
|
if samples and counts["HOME"] == 0 and counts["P_OT"] == 0 and counts["N_OT"] == 0:
|
|
print()
|
|
print("NOTE: no switch bit moved at all. If the travel above covers a full revolution,")
|
|
print(" the drive is not receiving a working switch signal — a broken wire on a")
|
|
print(" normally-closed fail-safe input reads exactly like a made switch. Check")
|
|
print(" the drive's DI wiring then, not the homing method.")
|
|
return 0
|
|
|
|
|
|
if __name__ == "__main__":
|
|
raise SystemExit(main())
|