fixed platform not moving problem
This commit is contained in:
@@ -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())
|
||||
Reference in New Issue
Block a user