orisys-tech/orisys-app
3
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'main' of ssh://892887cvmk08.aicp.io:24928/orisys-tech/orisys-app

Browse Source
This commit is contained in:
ykcheungab
2026-09-23 10:00:13 +08:00
parent 3347d17a20 b3279cc2a6
commit 4dad413d29
21 changed files with 1200 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/autocal/tools/home_trace.py
+219
View File
@@ -0,0 +1,219 @@
#!/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())
apps/autocal/tools/switch_probe.py
+187
View File
@@ -0,0 +1,187 @@
#!/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())