现场优化3

DeviceCommand/Devices/GantryControlTcp.cs

@@ -26,7 +26,7 @@ namespace DeviceCommand.Devices
         // =================================================================
         // ------------------ Coil / Bit 状态与控制 (位元件) ------------------
         // =================================================================
-        private const ushort ADDR_M_ESTOP = 20496;          // 0x5010: 紧停
+        private const ushort ADDR_M_ESTOP = 40;          // 0x5010: 紧停
         private const ushort ADDR_M_X_STOP = 41;            // 0x29: X轴停止
         private const ushort ADDR_M_Y_STOP = 42;            // 0x2A: Y轴停止
         private const ushort ADDR_M_Z_STOP = 43;            // 0x2B: Z轴停止
@@ -393,14 +393,26 @@ namespace DeviceCommand.Devices
             if (currentSpeed == 0)
                 throw new InvalidOperationException($"轴 {轴编号} 无法启动！当前设定速度为 0。");
 
-            // 3. 检查动作轴使能，若为 false 则自动补写 true 激活驱动器
-            bool isEnabled = await 查询轴使能(轴编号, 取消令牌);
-            if (!isEnabled)
+            // 3. 批量检查 1~6 号轴使能状态，若任意轴未使能则全部补写 true
+            bool[] allEnableStates = await ReadCoilsAsync(_slaveAddress, ADDR_HM_AXIS1_ENABLE, 6, 取消令牌);
+            if (allEnableStates == null || allEnableStates.Length < 6)
+                throw new InvalidOperationException("从 PLC 读取 1~6 号轴使能状态失败。");
+
+            bool hasModifiedEnable = false;
+            ushort[] enableAddresses = { ADDR_HM_AXIS1_ENABLE, ADDR_HM_AXIS2_ENABLE, ADDR_HM_AXIS3_ENABLE, ADDR_HM_AXIS4_ENABLE, ADDR_HM_AXIS5_ENABLE, ADDR_HM_AXIS6_ENABLE };
+
+            for (int i = 0; i < 6; i++)
             {
-                await 设置轴使能(轴编号, true, 取消令牌);
-                await Task.Delay(100, 取消令牌); // 等待硬件继电器响应与伺服驱动就绪
+                if (!allEnableStates[i])
+                {
+                    await WriteSingleCoilAsync(_slaveAddress, enableAddresses[i], true, 取消令牌);
+                    hasModifiedEnable = true;
+                }
             }
 
+            if (hasModifiedEnable)
+                await Task.Delay(100, 取消令牌); // 等待硬件继电器响应与伺服驱动就绪
+
             // 4. 精准独立校验当前需要动作轴的目标定位是否越界
             int currentTarget = await 查询轴目标位置(轴编号, 取消令牌);
             int allowedMaxLimit = maxLimit - currentHomePos;
@@ -568,5 +580,61 @@ namespace DeviceCommand.Devices
         }
 
         #endregion
+
+        #region 位置到达判定（超时轮询）
+
+        /// <summary>
+        /// 轮询等待三轴绝对位置均回到零点（容差 ±10），超时则抛出异常
+        /// </summary>
+        /// <param name="超时秒数">最大等待时间（秒）</param>
+        /// <param name="取消令牌">取消令牌</param>
+        public async Task 等待到达零点(int 超时秒数, CancellationToken 取消令牌 = default)
+        {
+            var deadline = DateTime.Now.AddSeconds(超时秒数);
+
+            while (DateTime.Now < deadline)
+            {
+                取消令牌.ThrowIfCancellationRequested();
+
+                var pos = await 查询全部轴绝对位置(取消令牌);
+                if (Math.Abs(pos.Axis1) <= 10 && Math.Abs(pos.Axis2) <= 10 && Math.Abs(pos.Axis3) <= 10)
+                    return;
+
+                await Task.Delay(100, 取消令牌);
+            }
+
+            throw new TimeoutException($"等待 {超时秒数} 秒后，三轴仍未回到零点");
+        }
+
+        /// <summary>
+        /// 轮询等待三轴绝对位置均到达各自设定的目标位置（容差 ±10），超时则抛出异常
+        /// </summary>
+        /// <param name="超时秒数">最大等待时间（秒）</param>
+        /// <param name="取消令牌">取消令牌</param>
+        public async Task 等待到达指定位置(int 超时秒数, CancellationToken 取消令牌 = default)
+        {
+            var targetX = await 查询轴目标位置(1, 取消令牌);
+            var targetY = await 查询轴目标位置(2, 取消令牌);
+            var targetZ = await 查询轴目标位置(3, 取消令牌);
+
+            var deadline = DateTime.Now.AddSeconds(超时秒数);
+
+            while (DateTime.Now < deadline)
+            {
+                取消令牌.ThrowIfCancellationRequested();
+
+                var pos = await 查询全部轴绝对位置(取消令牌);
+                if (Math.Abs(pos.Axis1 - targetX) <= 10
+                    && Math.Abs(pos.Axis2 - targetY) <= 10
+                    && Math.Abs(pos.Axis3 - targetZ) <= 10)
+                    return;
+
+                await Task.Delay(100, 取消令牌);
+            }
+
+            throw new TimeoutException($"等待 {超时秒数} 秒后，三轴仍未到达指定位置 (X:{targetX}, Y:{targetY}, Z:{targetZ})");
+        }
+
+        #endregion
     }
 }