huangsucan/ADP
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

现场优化4

Browse Source
This commit is contained in:
huangsucan
2026-08-15 18:43:38 +08:00
parent 8d941d92f0
commit 92eab2f626
5 changed files with 249 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
DeviceCommand/Base/ModbusTcp.cs
+2 -2
View File
@@ -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; }
DeviceCommand/Devices/GantryControlTcp.cs
+162 -24
View File
@@ -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;
// =================================================================
// ------------------ 软件位置跟踪（绝对位置寄存器会被清零）----------
// =================================================================
/// <summary>当前位置相对于机械零点的位置（每次移动后累加偏移）</summary>
private int _originX, _originY, _originZ;
/// <summary>是否已完成回零初始化</summary>
private bool _isHomed;
public GantryControlTcp(TcpConfig config) : base(config)
{
@@ -341,17 +350,70 @@ namespace DeviceCommand.Devices
}
/// <summary>
/// 全局回零（安全校验版：先检查回原点速度是否为 0，为0则拒绝触发）
/// 全局回零（安全校验版：先检查回原点速度是否为 0，为0则拒绝触发）。
/// 回零完成后读取绝对位置寄存器保存为初始值，重置累计偏移。
/// </summary>
/// <param name="回零点等待时间">回零点等待时间</param>
/// <param name="取消令牌">取消令牌</param>
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;
}
/// <summary>
@@ -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, 取消令牌);
}
/// <summary>
/// 三轴相对移动：以（初始绝对值 + 累计偏移 + 本次偏移）做越界校验，
/// 清零绝对位置寄存器后将偏移量写入目标寄存器，启动三轴联动，
/// 等待到达后清零目标寄存器并更新累计偏移。偏移量可为负值。
/// 调用前必须先执行回零()完成初始化。
/// </summary>
/// <param name="X偏移量">X轴相对偏移量（可为负）</param>
/// <param name="Y偏移量">Y轴相对偏移量（可为负）</param>
/// <param name="Z偏移量">Z轴相对偏移量（可为负）</param>
/// <param name="超时秒数">等待到达目标位置的最大超时时间（秒）</param>
/// <param name="取消令牌">取消令牌</param>
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 内部高低字工具转化方法
/// <summary>
@@ -582,12 +690,12 @@ namespace DeviceCommand.Devices
#endregion
#region 位置到达判定（超时轮询）
/// <summary>
/// 轮询等待三轴绝对位置均回到零点（容差 ±10），超时则抛出异常
/// 轮询等待三轴绝对位置均回到机械零点（容差 ±10），超时则抛出异常
/// </summary>
/// <param name="超时秒数">最大等待时间（秒）</param>
/// <param name="取消令牌">取消令牌</param>
[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($"等待 {超时秒数} 秒后，三轴仍未到达机械零点");
}
/// <summary>
/// 轮询等待三轴绝对位置到达指定的待定位置（容差 ±10），超时则抛出异常
/// </summary>
/// <param name="xTarget">X轴目标位置</param>
/// <param name="yTarget">Y轴目标位置</param>
/// <param name="zTarget">Z轴目标位置</param>
/// <param name="超时秒数">最大等待时间（秒）</param>
/// <param name="取消令牌">取消令牌</param>
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})");
}
/// <summary>
@@ -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, 取消令牌);
DeviceCommand/Devices/N69200.cs
+12
View File
@@ -336,6 +336,18 @@ namespace DeviceCommand.Devices
#region 3.6. 系统控制及报警清除
/// <summary>
/// 退出远程控制 (MEASure:EVENT?)
/// </summary>
/// <param name="是否开启">true 开启远程；false关闭远程</param>
/// <param name="ct">取消令牌</param>
public virtual async Task 启动远程控制(bool 是否开启, CancellationToken ct = default)
{
int val = 是否开启 ? 0 : 1;
await SendAsync($"SYSTem:EXIT:REMOte {val}{ScpiDelimiter}", ct);
}
/// <summary>
/// 读取负载报警/事件寄存器信息 (MEASure:EVENT?)
/// </summary>