using Common.Attributes; using DeviceCommand.Base; using Model.Models; using System; using System.Collections.Generic; using System.Linq; using System.Text; using System.Threading.Tasks; using System.Threading; namespace DeviceCommand.Devices { [ADPCommand] public class GantryControlTcp : ModbusTcp { private readonly byte _slaveAddress=1; // 向后兼容：TcpDevice 即自身（继承自 ModbusTcp） public ModbusTcp TcpDevice => this; public GantryControlTcp(TcpConfig config) : base(config) { } // ================================================================= // ------------------ Coil / Bit 状态与控制 (位元件) ------------------ // ================================================================= private const ushort ADDR_M_ESTOP = 20496; // 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轴停止 private const ushort ADDR_M_X_START = 30; // 0x1E: X轴指定位置启动 private const ushort ADDR_M_Y_START = 31; // 0x1F: Y轴指定位置启动 private const ushort ADDR_M_Z_START = 35; // 0x23: Z轴指定位置启动 private const ushort ADDR_M_HOME_TRIGGER = 100; // 0x64: 回原点触发 // ================================================================= // ------------------ 轴使能控制 (HM 寄存器) ------------------ // ================================================================= private const ushort ADDR_HM_AXIS1_ENABLE = 49408; // 0xC100: 轴1使能 (X轴) private const ushort ADDR_HM_AXIS2_ENABLE = 49409; // 0xC101: 轴2使能 (Y轴) private const ushort ADDR_HM_AXIS3_ENABLE = 49410; // 0xC102: 轴3使能 (Z轴) private const ushort ADDR_HM_AXIS4_ENABLE = 49411; // 0xC103: 轴4使能 private const ushort ADDR_HM_AXIS5_ENABLE = 49412; // 0xC104: 轴5使能 private const ushort ADDR_HM_AXIS6_ENABLE = 49413; // 0xC105: 轴6使能 // ================================================================= // ------------------ 速度与位置参数 (HD 32位数据寄存器) -------------- // ================================================================= private const ushort ADDR_HD_HOME_POS_SPEED = 41288; // 0xA148: 原点定位速度 (32位) // 轴原点位置 private const ushort ADDR_HD_X_HOME_POS = 41198; // 0xA0EE: X轴原点位 (32位) private const ushort ADDR_HD_Z_HOME_POS = 41208; // 0xA0F8: Z轴原点位 (32位) private const ushort ADDR_HD_Y_HOME_POS = 41218; // 0xA102: Y轴原点位 (32位) // 轴运行目标位置 private const ushort ADDR_HD_Y_TARGET = 41228; // 0xA10C: Y轴指定位置 (32位) private const ushort ADDR_HD_Z_TARGET = 41238; // 0xA116: Z轴指定位置 (32位) private const ushort ADDR_HD_X_TARGET = 41248; // 0xA120: X轴指定位置 (32位) // 轴运行速度 private const ushort ADDR_HD_X_SPEED = 41318; // 0xA166: X轴速度 (32位) private const ushort ADDR_HD_Z_SPEED = 41328; // 0xA170: Z轴速度 (32位) private const ushort ADDR_HD_Y_SPEED = 41338; // 0xA17A: Y轴速度 (32位) // ================================================================= // ------------------ 轴绝对位置（只读，相对原点）------------------ // ================================================================= private const ushort ADDR_ABS_AXIS1_POS = 47232; // 轴1绝对位置 (32位) private const ushort ADDR_ABS_AXIS2_POS = 47236; // 轴2绝对位置 (32位) private const ushort ADDR_ABS_AXIS3_POS = 47240; // 轴3绝对位置 (32位) private const ushort ADDR_ABS_AXIS4_POS = 47244; // 轴4绝对位置 (32位) private const ushort ADDR_ABS_AXIS5_POS = 47248; // 轴5绝对位置 (32位) private const ushort ADDR_ABS_AXIS6_POS = 47252; // 轴6绝对位置 (32位) // ================================================================= // ------------------ 核心控制逻辑与参数读写 ------------------------ // ================================================================= /// /// 轴使能控制 /// /// 轴编号 (1 - 6) public async Task SetAxisEnableAsync(int axis, bool enable, CancellationToken ct = default) { ushort address = axis switch { 1 => ADDR_HM_AXIS1_ENABLE, 2 => ADDR_HM_AXIS2_ENABLE, 3 => ADDR_HM_AXIS3_ENABLE, 4 => ADDR_HM_AXIS4_ENABLE, 5 => ADDR_HM_AXIS5_ENABLE, 6 => ADDR_HM_AXIS6_ENABLE, _ => throw new ArgumentException("轴通道错误，仅支持 1 - 6", nameof(axis)) }; await WriteSingleCoilAsync(_slaveAddress, address, enable, ct); } /// /// 读取指定轴的使能状态 /// /// 1: X轴, 2: Y轴, 3: Z轴 public async Task GetAxisEnableAsync(int axis, CancellationToken ct = default) { ushort address = axis switch { 1 => ADDR_HM_AXIS1_ENABLE, 2 => ADDR_HM_AXIS2_ENABLE, 3 => ADDR_HM_AXIS3_ENABLE, _ => throw new ArgumentException("轴通道错误，仅支持 1(X), 2(Y), 3(Z)", nameof(axis)) }; bool[] coils = await ReadCoilsAsync(_slaveAddress, address, 1, ct); return coils != null && coils.Length > 0 && coils[0]; } /// /// 同时设置 X/Y/Z 轴的原点位置（带各自独立的范围检查） /// /// X轴原点位置 (范围: 0 ~ 50000) /// Y轴原点位置 (范围: 0 ~ 50000) /// Z轴原点位置 (范围: 0 ~ 20000) public async Task SetHomePositionAsync(int xPos, int yPos, int zPos, CancellationToken ct = default) { if (xPos < 0 || xPos > 50000) throw new ArgumentOutOfRangeException(nameof(xPos), $"X轴原点设置值 [{xPos}] 超出合法范围 (0 - 50000)"); if (yPos < 0 || yPos > 50000) throw new ArgumentOutOfRangeException(nameof(yPos), $"Y轴原点设置值 [{yPos}] 超出合法范围 (0 - 50000)"); if (zPos < 0 || zPos > 20000) throw new ArgumentOutOfRangeException(nameof(zPos), $"Z轴原点设置值 [{zPos}] 超出合法范围 (0 - 20000)"); await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_X_HOME_POS, Int32ToUshorts(xPos), ct); await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_Y_HOME_POS, Int32ToUshorts(yPos), ct); await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_Z_HOME_POS, Int32ToUshorts(zPos), ct); } /// /// 批量查询当前 X、Y、Z 三轴设定的原点位置值 /// /// 返回包含三轴原点位置的元组 (X, Y, Z) public async Task<(int X, int Y, int Z)> GetHomePositionAsync(CancellationToken ct = default) { // 41218(Y) - 41198(X) = 20，加Y轴自身的 2 个寄存器，共批量读取 22 个连续寄存器 ushort[] registers = await ReadHoldingRegistersAsync(_slaveAddress, ADDR_HD_X_HOME_POS, 22, ct); if (registers == null || registers.Length < 22) throw new InvalidOperationException("从 PLC 读取三轴原点位置失败，返回数据长度不足。"); int xHomePos = UshortsToInt32(new ushort[] { registers[0], registers[1] }); int zHomePos = UshortsToInt32(new ushort[] { registers[10], registers[11] }); int yHomePos = UshortsToInt32(new ushort[] { registers[20], registers[21] }); return (xHomePos, yHomePos, zHomePos); } /// /// 设置回原点定位速度（带 0 - 100000 范围检查） /// public async Task SetHomePositionSpeedAsync(int speed, CancellationToken ct = default) { if (speed < 0 || speed > 100000) throw new ArgumentOutOfRangeException(nameof(speed), $"回原点定位速度值 [{speed}] 超出合法范围 (0 - 100000)"); await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_HOME_POS_SPEED, Int32ToUshorts(speed), ct); } /// /// 查询当前设定的回原点定位速度 /// public async Task GetHomePositionSpeedAsync(CancellationToken ct = default) { ushort[] registers = await ReadHoldingRegistersAsync(_slaveAddress, ADDR_HD_HOME_POS_SPEED, 2, ct); if (registers == null || registers.Length < 2) throw new InvalidOperationException("从 PLC 读取回原点定位速度失败，返回数据长度不足。"); return UshortsToInt32(registers); } /// /// 设置指定轴的运行速度（带 0 - 100000 范围检查） /// /// 1: X轴, 2: Y轴, 3: Z轴 public async Task SetAxisSpeedAsync(int axis, int speed, CancellationToken ct = default) { if (speed < 0 || speed > 100000) throw new ArgumentOutOfRangeException(nameof(speed), $"轴速度设置值 [{speed}] 超出合法范围 (0 - 100000)"); ushort address = axis switch { 1 => ADDR_HD_X_SPEED, 2 => ADDR_HD_Y_SPEED, 3 => ADDR_HD_Z_SPEED, _ => throw new ArgumentException("轴通道错误，仅支持 1(X), 2(Y), 3(Z)", nameof(axis)) }; await WriteMultipleRegistersAsync(_slaveAddress, address, Int32ToUshorts(speed), ct); } /// /// 读取指定轴当前设定的运行速度 /// public async Task GetAxisSpeedAsync(int axis, CancellationToken ct = default) { ushort address = axis switch { 1 => ADDR_HD_X_SPEED, 2 => ADDR_HD_Y_SPEED, 3 => ADDR_HD_Z_SPEED, _ => throw new ArgumentException("轴通道错误，仅支持 1(X), 2(Y), 3(Z)", nameof(axis)) }; ushort[] registers = await ReadHoldingRegistersAsync(_slaveAddress, address, 2, ct); if (registers == null || registers.Length < 2) throw new InvalidOperationException($"从 PLC 读取轴 {axis} 速度失败。"); return UshortsToInt32(registers); } /// /// 写入指定轴的目标位置（动态安全校验：目标位置不能超过 [轴最大物理极限 - 当前轴原点设置值]） /// public async Task SetAxisTargetPositionAsync(int axis, int targetPos, CancellationToken ct = default) { var (xHome, yHome, zHome) = await GetHomePositionAsync(ct); ushort targetAddress; int maxLimit; int currentHomePos; switch (axis) { case 1: targetAddress = ADDR_HD_X_TARGET; maxLimit = 50000; currentHomePos = xHome; break; case 2: targetAddress = ADDR_HD_Y_TARGET; maxLimit = 50000; currentHomePos = yHome; break; case 3: targetAddress = ADDR_HD_Z_TARGET; maxLimit = 20000; currentHomePos = zHome; break; default: throw new ArgumentException("轴通道错误，仅支持 1(X), 2(Y), 3(Z)", nameof(axis)); } int allowedMaxLimit = maxLimit - currentHomePos; if (targetPos < 0 || targetPos > allowedMaxLimit) { throw new ArgumentOutOfRangeException( nameof(targetPos), $"轴 {axis} 目标位置 [{targetPos}] 不合法！当前原点为 [{currentHomePos}]，允许的有效写入范围为: 0 - {allowedMaxLimit}" ); } await WriteMultipleRegistersAsync(_slaveAddress, targetAddress, Int32ToUshorts(targetPos), ct); } /// /// 读取指定轴当前设定的目标指定位置 /// public async Task GetAxisTargetPositionAsync(int axis, CancellationToken ct = default) { ushort address = axis switch { 1 => ADDR_HD_X_TARGET, 2 => ADDR_HD_Y_TARGET, 3 => ADDR_HD_Z_TARGET, _ => throw new ArgumentException("轴通道错误，仅支持 1(X), 2(Y), 3(Z)", nameof(axis)) }; ushort[] registers = await ReadHoldingRegistersAsync(_slaveAddress, address, 2, ct); if (registers == null || registers.Length < 2) throw new InvalidOperationException($"从 PLC 读取轴 {axis} 目标位置失败。"); return UshortsToInt32(registers); } /// /// 设置系统紧停锁定状态 (true: 锁定激活, false: 解除锁定) /// public async Task SetEStopStateAsync(bool state, CancellationToken ct = default) { await WriteSingleCoilAsync(_slaveAddress, ADDR_M_ESTOP, state, ct); } /// /// 设置指定轴的停止控制状态 (true: 持续输出停止信号, false: 释放停止信号) /// public async Task SetStopStateAsync(int axis, bool state, CancellationToken ct = default) { ushort address = axis switch { 1 => ADDR_M_X_STOP, 2 => ADDR_M_Y_STOP, 3 => ADDR_M_Z_STOP, _ => throw new ArgumentException("轴通道错误，仅支持 1(X), 2(Y), 3(Z)", nameof(axis)) }; await WriteSingleCoilAsync(_slaveAddress, address, state, ct); } /// /// 全局回零（安全校验版：先检查回原点速度是否为 0，为0则拒绝触发） /// public async Task HomeAsync(CancellationToken ct = default) { int currentSpeed = await GetHomePositionSpeedAsync(ct); if (currentSpeed == 0) throw new InvalidOperationException("无法触发回原点！当前回原点定位速度为 0，请先设置合法的回零速度。"); await TriggerCoilAsync(ADDR_M_HOME_TRIGGER, ct); } /// /// 单轴指定位置启动（含速度判定、使能自动补齐、动作目标位置独立范围校验） /// /// 1: X轴, 2: Y轴, 3: Z轴 public async Task StartSingleAxisAsync(int axis, CancellationToken ct = default) { ushort startAddress; int maxLimit; int currentHomePos; // 1. 获取基础参数与校验边界 var (xHome, yHome, zHome) = await GetHomePositionAsync(ct); switch (axis) { case 1: startAddress = ADDR_M_X_START; maxLimit = 50000; currentHomePos = xHome; break; case 2: startAddress = ADDR_M_Y_START; maxLimit = 50000; currentHomePos = yHome; break; case 3: startAddress = ADDR_M_Z_START; maxLimit = 20000; currentHomePos = zHome; break; default: throw new ArgumentException("轴通道错误，仅支持 1(X), 2(Y), 3(Z)", nameof(axis)); } // 2. 检查动作轴的速度 int currentSpeed = await GetAxisSpeedAsync(axis, ct); if (currentSpeed == 0) throw new InvalidOperationException($"轴 {axis} 无法启动！当前设定速度为 0。"); // 3. 检查动作轴使能，若为 false 则自动补写 true 激活驱动器 bool isEnabled = await GetAxisEnableAsync(axis, ct); if (!isEnabled) { await SetAxisEnableAsync(axis, true, ct); await Task.Delay(100, ct); // 等待硬件继电器响应与伺服驱动就绪 } // 4. 精准独立校验当前需要动作轴的目标定位是否越界 int currentTarget = await GetAxisTargetPositionAsync(axis, ct); int allowedMaxLimit = maxLimit - currentHomePos; if (currentTarget < 0 || currentTarget > allowedMaxLimit) { throw new ArgumentOutOfRangeException($"轴 {axis} 无法启动！当前指定目标位置 [{currentTarget}] 超过了允许的最大安全极限值 [{allowedMaxLimit}]。"); } // 5. 校验完成，边缘触发点动启动 await TriggerCoilAsync(startAddress, ct); } /// /// 三轴同时启动（全开：严格校验全部运行轴速度、批量检测与补正1~6号轴使能、联动校验三轴目标范围极限） /// public async Task StartAllAxesAsync(CancellationToken ct = default) { // 1. 一次性读取三轴速度，严禁任何一轴速度为 0 int xSpeed = await GetAxisSpeedAsync(1, ct); int ySpeed = await GetAxisSpeedAsync(2, ct); int zSpeed = await GetAxisSpeedAsync(3, ct); if (xSpeed == 0 || ySpeed == 0 || zSpeed == 0) throw new InvalidOperationException($"无法全开启动！存在运行速度为 0 的轴。当前速度 -> X:{xSpeed}, Y:{ySpeed}, Z:{zSpeed}"); // 2. 批量读取 1 ~ 6 号轴连续的使能状态线圈，极大地优化网络性能 bool[] enableStates = await ReadCoilsAsync(_slaveAddress, ADDR_HM_AXIS1_ENABLE, 6, ct); if (enableStates == null || enableStates.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++) { if (!enableStates[i]) { await WriteSingleCoilAsync(_slaveAddress, enableAddresses[i], true, ct); hasModifiedEnable = true; } } // 如果执行过使能补齐补齐写入，在循环外进行统一延时，保证PLC时序安全 if (hasModifiedEnable) await Task.Delay(100, ct); // 3. 获取三轴原点及当前设定的目标指定位置 var (xHome, yHome, zHome) = await GetHomePositionAsync(ct); int xTarget = await GetAxisTargetPositionAsync(1, ct); int yTarget = await GetAxisTargetPositionAsync(2, ct); int zTarget = await GetAxisTargetPositionAsync(3, ct); // 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, ct); await WriteSingleCoilAsync(_slaveAddress, ADDR_M_Y_START, true, ct); await WriteSingleCoilAsync(_slaveAddress, ADDR_M_Z_START, true, ct); await Task.Delay(100, ct); // 保持高电平状态以适配 PLC 扫描周期 await WriteSingleCoilAsync(_slaveAddress, ADDR_M_X_START, false, ct); await WriteSingleCoilAsync(_slaveAddress, ADDR_M_Y_START, false, ct); await WriteSingleCoilAsync(_slaveAddress, ADDR_M_Z_START, false, ct); } #region 内部高低字工具转化方法 /// /// 触发点动脉冲信号（置 1 后，等待 100ms 自动置 0 释放） /// private async Task TriggerCoilAsync(ushort address, CancellationToken ct = default) { await WriteSingleCoilAsync(_slaveAddress, address, true, ct); await Task.Delay(100, ct); await WriteSingleCoilAsync(_slaveAddress, address, false, ct); } /// /// 将 32 位整型数据转换为 Modbus 的 2 个 16 位无符号整数（低字在前 CDAB 格式） /// private ushort[] Int32ToUshorts(int value) { ushort lowWord = (ushort)(value & 0xFFFF); ushort highWord = (ushort)((value >> 16) & 0xFFFF); return new ushort[] { lowWord, highWord }; } /// /// 将 Modbus 的 2 个 16 位无符号整数转换为 32 位整型（低字在前 CDAB 格式） /// private int UshortsToInt32(ushort[] registers) { uint lowWord = registers[0]; uint highWord = registers[1]; return (int)((highWord << 16) | lowWord); } #endregion #region 轴绝对位置读取（只读） /// /// 读取指定轴的绝对位置（相对原点的实时位置，只读） /// /// 轴编号 (1-6) public async Task GetAbsolutePositionAsync(int axis, CancellationToken ct = default) { ushort address = axis switch { 1 => ADDR_ABS_AXIS1_POS, 2 => ADDR_ABS_AXIS2_POS, 3 => ADDR_ABS_AXIS3_POS, 4 => ADDR_ABS_AXIS4_POS, 5 => ADDR_ABS_AXIS5_POS, 6 => ADDR_ABS_AXIS6_POS, _ => throw new ArgumentException("轴编号错误，仅支持 1-6", nameof(axis)) }; ushort[] registers = await ReadHoldingRegistersAsync(_slaveAddress, address, 2, ct); if (registers == null || registers.Length < 2) throw new InvalidOperationException($"读取轴 {axis} 绝对位置失败。"); return UshortsToInt32(registers); } /// /// 批量读取 1-6 轴的绝对位置（一次性读取 22 个寄存器，高效） /// /// 返回 6 轴绝对位置元组 public async Task<(int Axis1, int Axis2, int Axis3, int Axis4, int Axis5, int Axis6)> GetAllAbsolutePositionsAsync(CancellationToken ct = default) { // 从 47232 开始连续读取 22 个寄存器（覆盖 47232-47253） ushort[] registers = await ReadHoldingRegistersAsync(_slaveAddress, ADDR_ABS_AXIS1_POS, 22, ct); if (registers == null || registers.Length < 22) throw new InvalidOperationException("批量读取 6 轴绝对位置失败，返回数据长度不足。"); // 每轴间隔 4 个寄存器（2个数据 + 2个间隙），提取各轴数据 int axis1 = UshortsToInt32(new ushort[] { registers[0], registers[1] }); int axis2 = UshortsToInt32(new ushort[] { registers[4], registers[5] }); int axis3 = UshortsToInt32(new ushort[] { registers[8], registers[9] }); int axis4 = UshortsToInt32(new ushort[] { registers[12], registers[13] }); int axis5 = UshortsToInt32(new ushort[] { registers[16], registers[17] }); int axis6 = UshortsToInt32(new ushort[] { registers[20], registers[21] }); return (axis1, axis2, axis3, axis4, axis5, axis6); } #endregion } }