@@ -0,0 +1,540 @@
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位)
// =================================================================
// ------------------ 核心控制逻辑与参数读写 ------------------------
// =================================================================
/// <summary>
/// 轴使能控制
/// </summary>
/// <param name="axis">轴编号 (1 - 6)</param>
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 ) ;
}
/// <summary>
/// 读取指定轴的使能状态
/// </summary>
/// <param name="axis">1: X轴, 2: Y轴, 3: Z轴</param>
public async Task < bool > 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 ] ;
}
/// <summary>
/// 同时设置 X/Y/Z 轴的原点位置(带各自独立的范围检查)
/// </summary>
/// <param name="xPos">X轴原点位置 (范围: 0 ~ 50000)</param>
/// <param name="yPos">Y轴原点位置 (范围: 0 ~ 50000)</param>
/// <param name="zPos">Z轴原点位置 (范围: 0 ~ 20000)</param>
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 ) ;
}
/// <summary>
/// 批量查询当前 X、Y、Z 三轴设定的原点位置值
/// </summary>
/// <returns>返回包含三轴原点位置的元组 (X, Y, Z)</returns>
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 ) ;
}
/// <summary>
/// 设置回原点定位速度(带 0 - 100000 范围检查)
/// </summary>
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 ) ;
}
/// <summary>
/// 查询当前设定的回原点定位速度
/// </summary>
public async Task < int > 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 ) ;
}
/// <summary>
/// 设置指定轴的运行速度(带 0 - 100000 范围检查)
/// </summary>
/// <param name="axis">1: X轴, 2: Y轴, 3: Z轴</param>
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 ) ;
}
/// <summary>
/// 读取指定轴当前设定的运行速度
/// </summary>
public async Task < int > 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 ) ;
}
/// <summary>
/// 写入指定轴的目标位置(动态安全校验:目标位置不能超过 [轴最大物理极限 - 当前轴原点设置值])
/// </summary>
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 ) ;
}
/// <summary>
/// 读取指定轴当前设定的目标指定位置
/// </summary>
public async Task < int > 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 ) ;
}
/// <summary>
/// 设置系统紧停锁定状态 (true: 锁定激活, false: 解除锁定)
/// </summary>
public async Task SetEStopStateAsync ( bool state , CancellationToken ct = default )
{
await WriteSingleCoilAsync ( _slaveAddress , ADDR_M_ESTOP , state , ct ) ;
}
/// <summary>
/// 设置指定轴的停止控制状态 (true: 持续输出停止信号, false: 释放停止信号)
/// </summary>
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 ) ;
}
/// <summary>
/// 全局回零(安全校验版:先检查回原点速度是否为 0, 为0则拒绝触发)
/// </summary>
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 ) ;
}
/// <summary>
/// 单轴指定位置启动(含速度判定、使能自动补齐、动作目标位置独立范围校验)
/// </summary>
/// <param name="axis">1: X轴, 2: Y轴, 3: Z轴</param>
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 ) ;
}
/// <summary>
/// 三轴同时启动( 全开: 严格校验全部运行轴速度、批量检测与补正1~6号轴使能、联动校验三轴目标范围极限)
/// </summary>
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 内 部 高 低 字 工 具 转 化 方 法
/// <summary>
/// 触发点动脉冲信号(置 1 后,等待 100ms 自动置 0 释放)
/// </summary>
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 ) ;
}
/// <summary>
/// 将 32 位整型数据转换为 Modbus 的 2 个 16 位无符号整数(低字在前 CDAB 格式)
/// </summary>
private ushort [ ] Int32ToUshorts ( int value )
{
ushort lowWord = ( ushort ) ( value & 0xFFFF ) ;
ushort highWord = ( ushort ) ( ( value > > 16 ) & 0xFFFF ) ;
return new ushort [ ] { lowWord , highWord } ;
}
/// <summary>
/// 将 Modbus 的 2 个 16 位无符号整数转换为 32 位整型(低字在前 CDAB 格式)
/// </summary>
private int UshortsToInt32 ( ushort [ ] registers )
{
uint lowWord = registers [ 0 ] ;
uint highWord = registers [ 1 ] ;
return ( int ) ( ( highWord < < 16 ) | lowWord ) ;
}
#endregion
#region 轴 绝 对 位 置 读 取 ( 只 读 )
/// <summary>
/// 读取指定轴的绝对位置(相对原点的实时位置,只读)
/// </summary>
/// <param name="axis">轴编号 (1-6)</param>
public async Task < int > 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 ) ;
}
/// <summary>
/// 批量读取 1-6 轴的绝对位置(一次性读取 22 个寄存器,高效)
/// </summary>
/// <returns>返回 6 轴绝对位置元组</returns>
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
}
}