From 92eab2f626f16b824b38fc07d0d6cab8f103a440 Mon Sep 17 00:00:00 2001 From: hsc Date: Sat, 15 Aug 2026 12:52:55 +0800 Subject: [PATCH] =?UTF-8?q?=E7=8E=B0=E5=9C=BA=E4=BC=98=E5=8C=964?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../ViewModels/BenchMovementViewModel.cs | 2 +- DeviceCommand/Base/ModbusTcp.cs | 4 +- DeviceCommand/Devices/GantryControlTcp.cs | 186 +++++++++++++++--- DeviceCommand/Devices/N69200.cs | 12 ++ ZLGUSBCANFD/USBCANFD.cs | 76 ++++++- 5 files changed, 249 insertions(+), 31 deletions(-) diff --git a/BenchMovementModule/ViewModels/BenchMovementViewModel.cs b/BenchMovementModule/ViewModels/BenchMovementViewModel.cs index 6560420..f13aaf9 100644 --- a/BenchMovementModule/ViewModels/BenchMovementViewModel.cs +++ b/BenchMovementModule/ViewModels/BenchMovementViewModel.cs @@ -287,7 +287,7 @@ namespace BenchMovementModule.ViewModels if (!EnsureConnected()) return; try { - await _gantry!.回零(); + await _gantry!.回零(15); LoggerHelper.Info("[BenchMovement] 回原点已触发"); } catch (Exception ex) diff --git a/DeviceCommand/Base/ModbusTcp.cs b/DeviceCommand/Base/ModbusTcp.cs index 1e370df..0b2c27e 100644 --- a/DeviceCommand/Base/ModbusTcp.cs +++ b/DeviceCommand/Base/ModbusTcp.cs @@ -12,8 +12,8 @@ namespace DeviceCommand.Base { public string IPAddress { get; private set; } = "127.0.0.1"; public int Port { get; private set; } = 502; - public int SendTimeout { get; private set; } = 3000; - public int ReceiveTimeout { get; private set; } = 3000; + public int SendTimeout { get; private set; } = 10000; + public int ReceiveTimeout { get; private set; } = 10000; private TcpClient _tcpClient; public IModbusMaster Modbus { get; private set; } diff --git a/DeviceCommand/Devices/GantryControlTcp.cs b/DeviceCommand/Devices/GantryControlTcp.cs index 1350fd7..b82a11d 100644 --- a/DeviceCommand/Devices/GantryControlTcp.cs +++ b/DeviceCommand/Devices/GantryControlTcp.cs @@ -7,6 +7,7 @@ using System.Linq; using System.Text; using System.Threading.Tasks; using System.Threading; +using System.ComponentModel; namespace DeviceCommand.Devices { @@ -18,6 +19,14 @@ namespace DeviceCommand.Devices // 向后兼容：TcpDevice 即自身（继承自 ModbusTcp） public ModbusTcp TcpDevice => this; + // ================================================================= + // ------------------ 软件位置跟踪（绝对位置寄存器会被清零）---------- + // ================================================================= + /// 当前位置相对于机械零点的位置（每次移动后累加偏移） + private int _originX, _originY, _originZ; + /// 是否已完成回零初始化 + private bool _isHomed; + public GantryControlTcp(TcpConfig config) : base(config) { @@ -341,17 +350,70 @@ namespace DeviceCommand.Devices } /// - /// 全局回零（安全校验版：先检查回原点速度是否为 0，为0则拒绝触发） + /// 全局回零（安全校验版：先检查回原点速度是否为 0，为0则拒绝触发）。 + /// 回零完成后读取绝对位置寄存器保存为初始值，重置累计偏移。 /// + /// 回零点等待时间 /// 取消令牌 - public async Task 回零(CancellationToken 取消令牌 = default) + public async Task 回零(int 回零点等待时间, CancellationToken 取消令牌 = default) { int currentSpeed = await 查询回零速度(取消令牌); if (currentSpeed == 0) throw new InvalidOperationException("无法触发回原点！当前回原点定位速度为 0，请先设置合法的回零速度。"); + // 1. 触发回零 await TriggerCoilAsync(ADDR_M_HOME_TRIGGER, 取消令牌); + + // 2. 等待到达机械零点（绝对位置 = 0） + await 等待到达零点(回零点等待时间, 取消令牌); + await Task.Delay(500, 取消令牌); + + // 3. 清零绝对位置寄存器 + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS1_POS, Int32ToUshorts(0), 取消令牌); + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS2_POS, Int32ToUshorts(0), 取消令牌); + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS3_POS, Int32ToUshorts(0), 取消令牌); + + // 4. 设置当前位置为机械零点 (0, 0, 0) + // 设置目标位置 + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_X_TARGET, Int32ToUshorts(0), 取消令牌); + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_Y_TARGET, Int32ToUshorts(0), 取消令牌); + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_Z_TARGET, Int32ToUshorts(0), 取消令牌); + + // 5. 读取配置的待定位置（原点位置） + var (xHome, yHome, zHome) = await 查询原点位置(取消令牌); + + // 6. 移动到待定位置 + if (xHome != 0 || yHome != 0 || zHome != 0) + { + + _originX = xHome; + _originY = yHome; + _originZ = zHome; + + // 启动三轴联动 + await 启动全部轴(取消令牌); + + // 等待到达待定位置 + await 等待到达待定位置(xHome, yHome, zHome, 回零点等待时间, 取消令牌); + + // 7. 到达后清零绝对位置寄存器 + await Task.Delay(500, 取消令牌); + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS1_POS, Int32ToUshorts(0), 取消令牌); + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS2_POS, Int32ToUshorts(0), 取消令牌); + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS3_POS, Int32ToUshorts(0), 取消令牌); + + // 清零目标寄存器 + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_X_TARGET, Int32ToUshorts(0), 取消令牌); + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_Y_TARGET, Int32ToUshorts(0), 取消令牌); + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_Z_TARGET, Int32ToUshorts(0), 取消令牌); + } + + // 8. 更新当前位置为待定位置 + _originX = xHome; + _originY = yHome; + _originZ = zHome; + _isHomed = true; } /// @@ -467,22 +529,6 @@ namespace DeviceCommand.Devices int yTarget = await 查询轴目标位置(2, 取消令牌); int zTarget = await 查询轴目标位置(3, 取消令牌); - // X轴安全范围联动校验 - int xMaxAllowed = 50000 - xHome; - if (xTarget < 0 || xTarget > xMaxAllowed) - throw new ArgumentOutOfRangeException(nameof(xTarget), $"X轴目标位置 [{xTarget}] 越界，当前原点下最大范围 0-{xMaxAllowed}"); - - // Y轴安全范围联动校验 - int yMaxAllowed = 50000 - yHome; - if (yTarget < 0 || yTarget > yMaxAllowed) - throw new ArgumentOutOfRangeException(nameof(yTarget), $"Y轴目标位置 [{yTarget}] 越界，当前原点下最大范围 0-{yMaxAllowed}"); - - // Z轴安全范围联动校验 - int zMaxAllowed = 20000 - zHome; - if (zTarget < 0 || zTarget > zMaxAllowed) - throw new ArgumentOutOfRangeException(nameof(zTarget), $"Z轴目标位置 [{zTarget}] 越界，当前原点下最大范围 0-{zMaxAllowed}"); - - // 4. 全部联动校验通过，三轴点动同步启动 await WriteSingleCoilAsync(_slaveAddress, ADDR_M_X_START, true, 取消令牌); await WriteSingleCoilAsync(_slaveAddress, ADDR_M_Y_START, true, 取消令牌); await WriteSingleCoilAsync(_slaveAddress, ADDR_M_Z_START, true, 取消令牌); @@ -494,6 +540,68 @@ namespace DeviceCommand.Devices await WriteSingleCoilAsync(_slaveAddress, ADDR_M_Z_START, false, 取消令牌); } + /// + /// 三轴相对移动：以（初始绝对值 + 累计偏移 + 本次偏移）做越界校验， + /// 清零绝对位置寄存器后将偏移量写入目标寄存器，启动三轴联动， + /// 等待到达后清零目标寄存器并更新累计偏移。偏移量可为负值。 + /// 调用前必须先执行回零()完成初始化。 + /// + /// X轴相对偏移量（可为负） + /// Y轴相对偏移量（可为负） + /// Z轴相对偏移量（可为负） + /// 等待到达目标位置的最大超时时间（秒） + /// 取消令牌 + public async Task 相对移动(int X偏移量, int Y偏移量, int Z偏移量, int 超时秒数 = 60, CancellationToken 取消令牌 = default) + { + if (!_isHomed) + throw new InvalidOperationException("尚未完成回零初始化，请先调用 回零() 方法。"); + + // 1. 越界校验：当前位置 + 本次偏移是否在合法范围 + int targetAbsX = _originX + X偏移量; + int targetAbsY = _originY + Y偏移量; + int targetAbsZ = _originZ + Z偏移量; + + if (targetAbsX > 50000) + throw new ArgumentOutOfRangeException(nameof(X偏移量), + $"X轴越界：当前 {_originX} + 偏移 {X偏移量} = {targetAbsX}，最大范围 50000"); + + if (targetAbsY > 50000) + throw new ArgumentOutOfRangeException(nameof(Y偏移量), + $"Y轴越界：当前 {_originY} + 偏移 {Y偏移量} = {targetAbsY}，最大范围 50000"); + + if (targetAbsZ > 20000) + throw new ArgumentOutOfRangeException(nameof(Z偏移量), + $"Z轴越界：当前 {_originZ} + 偏移 {Z偏移量} = {targetAbsZ}，最大范围 20000"); + + // 2. 将偏移量写入目标位置寄存器（ABS已在上次移动后清零，目标 = 偏移量） + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_X_TARGET, Int32ToUshorts(X偏移量), 取消令牌); + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_Y_TARGET, Int32ToUshorts(Y偏移量), 取消令牌); + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_Z_TARGET, Int32ToUshorts(Z偏移量), 取消令牌); + + // 3. 启动三轴联动 + await 启动全部轴(取消令牌); + await 等待到达指定位置(超时秒数); + await 清空绝对寄存器(); + + // 5. 到达后更新当前位置（累加偏移） + _originX += X偏移量; + _originY += Y偏移量; + _originZ += Z偏移量; + + + + + } + public async Task 清空绝对寄存器( CancellationToken 取消令牌 = default) + { + // 6. 清零 6 轴绝对位置寄存器 + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS1_POS, Int32ToUshorts(0), 取消令牌); + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS2_POS, Int32ToUshorts(0), 取消令牌); + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS3_POS, Int32ToUshorts(0), 取消令牌); + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS4_POS, Int32ToUshorts(0), 取消令牌); + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS5_POS, Int32ToUshorts(0), 取消令牌); + await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS6_POS, Int32ToUshorts(0), 取消令牌); + } #region 内部高低字工具转化方法 /// @@ -582,12 +690,12 @@ namespace DeviceCommand.Devices #endregion #region 位置到达判定（超时轮询） - /// - /// 轮询等待三轴绝对位置均回到零点（容差 ±10），超时则抛出异常 + /// 轮询等待三轴绝对位置均回到机械零点（容差 ±10），超时则抛出异常 /// /// 最大等待时间（秒） /// 取消令牌 + [Browsable(false)] public async Task 等待到达零点(int 超时秒数, CancellationToken 取消令牌 = default) { var deadline = DateTime.Now.AddSeconds(超时秒数); @@ -597,13 +705,43 @@ namespace DeviceCommand.Devices 取消令牌.ThrowIfCancellationRequested(); var pos = await 查询全部轴绝对位置(取消令牌); - if (Math.Abs(pos.Axis1) <= 10 && Math.Abs(pos.Axis2) <= 10 && Math.Abs(pos.Axis3) <= 10) + if (Math.Abs(pos.Axis1) <= 10 + && Math.Abs(pos.Axis6) <= 10 + && Math.Abs(pos.Axis2) <= 10) return; await Task.Delay(100, 取消令牌); } - throw new TimeoutException($"等待 {超时秒数} 秒后，三轴仍未回到零点"); + throw new TimeoutException($"等待 {超时秒数} 秒后，三轴仍未到达机械零点"); + } + + /// + /// 轮询等待三轴绝对位置到达指定的待定位置（容差 ±10），超时则抛出异常 + /// + /// X轴目标位置 + /// Y轴目标位置 + /// Z轴目标位置 + /// 最大等待时间（秒） + /// 取消令牌 + private async Task 等待到达待定位置(int xTarget, int yTarget, int zTarget, int 超时秒数, CancellationToken 取消令牌 = default) + { + var deadline = DateTime.Now.AddSeconds(超时秒数); + + while (DateTime.Now < deadline) + { + 取消令牌.ThrowIfCancellationRequested(); + + var pos = await 查询全部轴绝对位置(取消令牌); + if (Math.Abs(pos.Axis1 - xTarget) <= 10 + && Math.Abs(pos.Axis6 - yTarget) <= 10 + && Math.Abs(pos.Axis2 - zTarget) <= 10) + return; + + await Task.Delay(100, 取消令牌); + } + + throw new TimeoutException($"等待 {超时秒数} 秒后，三轴仍未到达待定位置 (X:{xTarget}, Y:{yTarget}, Z:{zTarget})"); } /// @@ -625,8 +763,8 @@ namespace DeviceCommand.Devices var pos = await 查询全部轴绝对位置(取消令牌); if (Math.Abs(pos.Axis1 - targetX) <= 10 - && Math.Abs(pos.Axis2 - targetY) <= 10 - && Math.Abs(pos.Axis3 - targetZ) <= 10) + && Math.Abs(pos.Axis6 - targetY) <= 10 + && Math.Abs(pos.Axis2 - targetZ) <= 10) return; await Task.Delay(100, 取消令牌); diff --git a/DeviceCommand/Devices/N69200.cs b/DeviceCommand/Devices/N69200.cs index 8002ae6..652715b 100644 --- a/DeviceCommand/Devices/N69200.cs +++ b/DeviceCommand/Devices/N69200.cs @@ -336,6 +336,18 @@ namespace DeviceCommand.Devices #region 3.6. 系统控制及报警清除 + + /// + /// 退出远程控制 (MEASure:EVENT?) + /// + /// true 开启远程；false关闭远程 + /// 取消令牌 + + public virtual async Task 启动远程控制(bool 是否开启, CancellationToken ct = default) + { + int val = 是否开启 ? 0 : 1; + await SendAsync($"SYSTem:EXIT:REMOte {val}{ScpiDelimiter}", ct); + } /// /// 读取负载报警/事件寄存器信息 (MEASure:EVENT?) /// diff --git a/ZLGUSBCANFD/USBCANFD.cs b/ZLGUSBCANFD/USBCANFD.cs index be70fe9..7e3dba4 100644 --- a/ZLGUSBCANFD/USBCANFD.cs +++ b/ZLGUSBCANFD/USBCANFD.cs @@ -688,6 +688,7 @@ namespace ZLGUSBCANFD /// /// 发送自定义报文（原始字节，帧 ID 支持 string 兼容模式）。 + /// 循环发送间隔毫秒 = 0 时只发送一次；>0 时按指定间隔循环发送，可调用 停止循环发送 停止。 /// /// CAN/CANFD 通道索引 /// 帧 ID（可以是 "26"、"0x1A"、"1A"）；扩展帧会自动置位 0x80000000 @@ -696,6 +697,7 @@ namespace ZLGUSBCANFD /// 是否为 29 位扩展帧 /// true = CANFD；false = 经典 CAN /// CANFD 是否开启波特率加速 + /// 0 = 单次发送；>0 = 周期循环发送（毫秒） public virtual bool 发送自定义报文( uint 通道号, string 帧IDStr, @@ -703,7 +705,8 @@ namespace ZLGUSBCANFD byte dlcLength, bool 是否是扩展帧 = false, bool 是否是CANFD = true, - bool 开启波特率加速BRS = true) + bool 开启波特率加速BRS = true, + int 循环发送间隔毫秒 = 0) { if (!TryParseFrameId(帧IDStr, out uint 帧ID)) { @@ -713,23 +716,88 @@ namespace ZLGUSBCANFD if (通道号 >= _maxChannels || _channelHandles[通道号] == IntPtr.Zero) return false; if (原始数据 == null) return false; + if (循环发送间隔毫秒 < 0) throw new ArgumentOutOfRangeException(nameof(循环发送间隔毫秒), "循环发送间隔必须大于或等于 0。"); // 扩展帧需要把最高位标志位置起来 uint canId = 帧ID & 0x7FFFFFFF; if (是否是扩展帧) canId |= 0x80000000; + // 单次发送 + if (循环发送间隔毫秒 == 0) + { + return 发送自定义报文单次(通道号, canId, 原始数据, dlcLength, 是否是扩展帧, 是否是CANFD, 开启波特率加速BRS); + } + + // 循环发送：先停止同通道同帧 ID 的旧循环，再启动新循环 + 停止循环发送(通道号, 帧ID); + + // 拷贝原始数据，避免外部修改影响循环发送 + byte[] dataCopy = new byte[原始数据.Length]; + Array.Copy(原始数据, dataCopy, 原始数据.Length); + + var cts = new CancellationTokenSource(); + var sendTask = Task.Run(async () => + { + while (!cts.Token.IsCancellationRequested) + { + if (_isClosing) break; + try + { + lock (_channelLocks[通道号]) + { + if (_channelHandles[通道号] == IntPtr.Zero || _isClosing) break; + 发送自定义报文单次(通道号, canId, dataCopy, dlcLength, 是否是扩展帧, 是否是CANFD, 开启波特率加速BRS); + } + + await Task.Delay(循环发送间隔毫秒, cts.Token); + } + catch (OperationCanceledException) + { + break; + } + catch (Exception ex) + { + Logger.LoggerHelper.Error($"[ZLGCANFD] 循环发送自定义报文失败: {ex.Message}"); + break; + } + } + + if (_cyclicSenders.TryGetValue((通道号, 帧ID), out var current) && ReferenceEquals(current.Cts, cts)) + { + _cyclicSenders.TryRemove((通道号, 帧ID), out _); + } + cts.Dispose(); + }, cts.Token); + + _cyclicSenders[(通道号, 帧ID)] = (cts, sendTask); + + return true; + } + + /// + /// 发送自定义报文的单次执行（内部方法，无循环逻辑）。 + /// + [Browsable(false)] + private bool 发送自定义报文单次( + uint 通道号, + uint canId, + byte[] 原始数据, + byte dlcLength, + bool 是否是扩展帧, + bool 是否是CANFD, + bool 开启波特率加速BRS) + { lock (_channelLocks[通道号]) { if (是否是CANFD) { - // CANFD 有效 DLC 校验 byte validFdDlc = 校验CANFD的Dlc(dlcLength); ZLGCAN.canfd_frame fdFrame = new ZLGCAN.canfd_frame { can_id = canId, len = validFdDlc, - flags = (byte)((是否是扩展帧 ? 0x01 : 0x00) | (开启波特率加速BRS ? 0x02 : 0x00) | 0x20), // 0x20 本地回显 + flags = (byte)((是否是扩展帧 ? 0x01 : 0x00) | (开启波特率加速BRS ? 0x02 : 0x00) | 0x20), data = new byte[64] }; Array.Copy(原始数据, 0, fdFrame.data, 0, Math.Min(原始数据.Length, 64)); @@ -749,7 +817,7 @@ namespace ZLGUSBCANFD { can_id = canId, can_dlc = dlcLength > 8 ? (byte)8 : dlcLength, - __pad = 0x20, // 发送回显 + __pad = 0x20, data = new byte[8] }; Array.Copy(原始数据, 0, standardFrame.data, 0, Math.Min(原始数据.Length, 8));