空回微抬改成一条厂家 MoveL,并按实测抬高降回。默认 Z50 会在还差约 0.5mm 时就报到位,微升降改用 ZFine 和到位等级 1。合到 0 时固件常报 grip=1,手指已在合拢端就继续顺时针,避免最后一转永远不发。快转样条按峰速不超过 0.90 rad/s 排程,避免时间到了还差几十度。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -18,15 +18,34 @@ ACK = Path("/tmp/flexiv_movel_ack")
|
||||
DONE = Path("/tmp/flexiv_movel_done")
|
||||
|
||||
|
||||
def format_request(seq: int, x: float, y: float, z: float, keep_z: bool, vel: float) -> str:
|
||||
return (
|
||||
def format_request(
|
||||
seq: int, x: float, y: float, z: float, keep_z: bool, vel: float,
|
||||
zone: str = "", toler: int = -1,
|
||||
) -> str:
|
||||
"""一行请求。不带 ``zone`` 时仍是 6 个数,插孔和粗定位保持厂家默认到位。
|
||||
|
||||
微升降要带过渡区和到位等级。厂家默认 ``Z50``(50 mm)加等级 3,
|
||||
几毫米的直线会在还差约 0.5 mm 时就报到位。合法的最细过渡区是 ``ZFine``,
|
||||
最严且仍会检查到位的等级是 1(0 表示不检查)。
|
||||
"""
|
||||
if bool(zone) != (int(toler) >= 0):
|
||||
raise ValueError("MoveL 过渡区和到位等级必须一起给")
|
||||
if zone and (zone != "ZFine" or int(toler) != 1):
|
||||
raise ValueError(f"MoveL 微升降只接受 ZFine 和到位等级 1,收到 {zone} {toler}")
|
||||
line = (
|
||||
f"{int(seq)} {float(x):.6f} {float(y):.6f} {float(z):.6f} "
|
||||
f"{1 if keep_z else 0} {float(vel):.4f}\n"
|
||||
f"{1 if keep_z else 0} {float(vel):.4f}"
|
||||
)
|
||||
if zone:
|
||||
line += f" {zone} {int(toler)}"
|
||||
return line + "\n"
|
||||
|
||||
|
||||
def write_request(seq: int, x: float, y: float, z: float, keep_z: bool, vel: float) -> None:
|
||||
text = format_request(seq, x, y, z, keep_z, vel)
|
||||
def write_request(
|
||||
seq: int, x: float, y: float, z: float, keep_z: bool, vel: float,
|
||||
zone: str = "", toler: int = -1,
|
||||
) -> None:
|
||||
text = format_request(seq, x, y, z, keep_z, vel, zone, toler)
|
||||
tmp = REQ.with_name(REQ.name + ".tmp")
|
||||
tmp.write_text(text, encoding="utf-8")
|
||||
tmp.replace(REQ)
|
||||
|
||||
Reference in New Issue
Block a user