将Feature/XP.Common和Feature/XP.Hardware分支合并至Develop/XP.forHardwareAndCommon，完善XPapp注册和相关硬件类库通用类库功能。
This commit is contained in:
QI Mingxuan
@@ -0,0 +1,770 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Threading;
|
||||
using System.Windows;
|
||||
using Prism.Events;
|
||||
using XP.Common.Localization;
|
||||
using XP.Common.Logging.Interfaces;
|
||||
using XP.Hardware.MotionControl.Abstractions;
|
||||
using XP.Hardware.MotionControl.Abstractions.Enums;
|
||||
using XP.Hardware.MotionControl.Abstractions.Events;
|
||||
using XP.Hardware.MotionControl.Config;
|
||||
using XP.Hardware.Plc.Abstractions;
|
||||
|
||||
namespace XP.Hardware.MotionControl.Services
|
||||
{
|
||||
/// <summary>
|
||||
/// 运动控制业务服务实现 | Motion Control Business Service Implementation
|
||||
/// 封装所有运动控制业务规则，包括单轴/多轴移动、Jog控制、安全门控制和几何计算
|
||||
/// Encapsulates all motion control business rules including single/multi-axis move, jog control, safety door control and geometry calculation
|
||||
/// </summary>
|
||||
public class MotionControlService : IMotionControlService
|
||||
{
|
||||
private readonly IMotionSystem _motionSystem;
|
||||
private readonly GeometryCalculator _geometryCalculator;
|
||||
private readonly MotionControlConfig _config;
|
||||
private readonly IEventAggregator _eventAggregator;
|
||||
private readonly ILoggerService _logger;
|
||||
private readonly IPlcService _plcService;
|
||||
|
||||
// 轮询定时器 | Polling timer
|
||||
private Timer _pollingTimer;
|
||||
private int _pollErrorCount = 0;
|
||||
|
||||
/// <summary>
|
||||
/// 安全执行包装：捕获 PLC 异常避免崩溃 | Safe execution wrapper: catches PLC exceptions to prevent crash
|
||||
/// </summary>
|
||||
private MotionResult SafeExecute(Func<MotionResult> action, string operationName)
|
||||
{
|
||||
try
|
||||
{
|
||||
return action();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.Error(ex, "{Operation} 执行异常 | {Operation} execution error: {Message}", operationName, operationName, ex.Message);
|
||||
return MotionResult.Fail($"{operationName} 异常: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造函数 | Constructor
|
||||
/// 注入所有依赖项 | Inject all dependencies
|
||||
/// </summary>
|
||||
public MotionControlService(
|
||||
IMotionSystem motionSystem,
|
||||
GeometryCalculator geometryCalculator,
|
||||
MotionControlConfig config,
|
||||
IEventAggregator eventAggregator,
|
||||
ILoggerService logger,
|
||||
IPlcService plcService)
|
||||
{
|
||||
_motionSystem = motionSystem ?? throw new ArgumentNullException(nameof(motionSystem));
|
||||
_geometryCalculator = geometryCalculator ?? throw new ArgumentNullException(nameof(geometryCalculator));
|
||||
_config = config ?? throw new ArgumentNullException(nameof(config));
|
||||
_eventAggregator = eventAggregator ?? throw new ArgumentNullException(nameof(eventAggregator));
|
||||
// 使用模块化日志实例 | Use module-scoped logger instance
|
||||
_logger = (logger ?? throw new ArgumentNullException(nameof(logger)))
|
||||
.ForModule<MotionControlService>();
|
||||
_plcService = plcService ?? throw new ArgumentNullException(nameof(plcService));
|
||||
}
|
||||
|
||||
#region 轮询控制 | Polling Control
|
||||
|
||||
/// <summary>
|
||||
/// 启动 PLC 状态轮询 | Start PLC status polling
|
||||
/// 使用 System.Threading.Timer 以配置的 PollingInterval 周期执行轮询
|
||||
/// Uses System.Threading.Timer with configured PollingInterval for periodic polling
|
||||
/// </summary>
|
||||
public void StartPolling()
|
||||
{
|
||||
_pollingTimer?.Dispose();
|
||||
_pollingTimer = new Timer(OnPollingTick, null, 0, _config.PollingInterval);
|
||||
_logger.Info("轮询已启动，周期={Interval}ms | Polling started, interval={Interval}ms", _config.PollingInterval);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 停止 PLC 状态轮询 | Stop PLC status polling
|
||||
/// </summary>
|
||||
public void StopPolling()
|
||||
{
|
||||
_pollingTimer?.Dispose();
|
||||
_pollingTimer = null;
|
||||
_logger.Info("轮询已停止 | Polling stopped");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 轮询回调方法 | Polling tick callback
|
||||
/// 更新所有轴状态、重新计算几何参数、检测状态变化并发布事件
|
||||
/// Updates all axis status, recalculates geometry, detects status changes and publishes events
|
||||
/// </summary>
|
||||
private void OnPollingTick(object state)
|
||||
{
|
||||
// PLC 未连接时跳过轮询 | Skip polling when PLC is not connected
|
||||
if (!_plcService.IsConnected) return;
|
||||
|
||||
// 连续错误过多时降频：每50次轮询才尝试一次 | Throttle when too many consecutive errors
|
||||
if (_pollErrorCount > 3)
|
||||
{
|
||||
// 仅递增计数，不执行轮询 | Only increment count, skip polling
|
||||
if (++_pollErrorCount % 50 != 0) return;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
// 缓存旧的直线轴状态用于变化检测 | Cache old linear axis statuses for change detection
|
||||
var oldLinearStatuses = new Dictionary<AxisId, AxisStatus>();
|
||||
foreach (var kvp in _motionSystem.LinearAxes)
|
||||
oldLinearStatuses[kvp.Key] = kvp.Value.Status;
|
||||
|
||||
// 缓存旧的旋转轴状态 | Cache old rotary axis statuses
|
||||
var oldRotaryStatuses = new Dictionary<RotaryAxisId, AxisStatus>();
|
||||
foreach (var kvp in _motionSystem.RotaryAxes)
|
||||
oldRotaryStatuses[kvp.Key] = kvp.Value.Status;
|
||||
|
||||
// 缓存旧的门状态 | Cache old door status
|
||||
var oldDoorStatus = _motionSystem.SafetyDoor.Status;
|
||||
var oldInterlocked = _motionSystem.SafetyDoor.IsInterlocked;
|
||||
|
||||
// 更新所有轴和门状态 | Update all axis and door status
|
||||
_motionSystem.UpdateAllStatus();
|
||||
|
||||
// 重新计算几何参数并发布事件 | Recalculate geometry and publish event
|
||||
var geometry = GetCurrentGeometry();
|
||||
_eventAggregator.GetEvent<GeometryUpdatedEvent>()
|
||||
.Publish(new GeometryData(geometry.FOD, geometry.FDD, geometry.Magnification));
|
||||
|
||||
// 检测直线轴状态变化 | Detect linear axis status changes
|
||||
foreach (var kvp in _motionSystem.LinearAxes)
|
||||
{
|
||||
if (oldLinearStatuses.TryGetValue(kvp.Key, out var oldStatus) && oldStatus != kvp.Value.Status)
|
||||
{
|
||||
_eventAggregator.GetEvent<AxisStatusChangedEvent>()
|
||||
.Publish(new AxisStatusChangedData(kvp.Key, kvp.Value.Status));
|
||||
|
||||
// 轴状态变为 Error/Alarm 时同时发布 MotionErrorEvent | Publish MotionErrorEvent when status becomes Error/Alarm
|
||||
if (kvp.Value.Status == AxisStatus.Error || kvp.Value.Status == AxisStatus.Alarm)
|
||||
{
|
||||
_eventAggregator.GetEvent<MotionErrorEvent>()
|
||||
.Publish(new MotionErrorData(kvp.Key, $"直线轴 {kvp.Key} 状态变为 {kvp.Value.Status} | Linear axis {kvp.Key} status changed to {kvp.Value.Status}"));
|
||||
_logger.Error(null, "直线轴 {AxisId} 状态变为 {Status} | Linear axis {AxisId} status changed to {Status}", kvp.Key, kvp.Value.Status);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 检测旋转轴状态变化 | Detect rotary axis status changes
|
||||
foreach (var kvp in _motionSystem.RotaryAxes)
|
||||
{
|
||||
if (oldRotaryStatuses.TryGetValue(kvp.Key, out var oldStatus) && oldStatus != kvp.Value.Status)
|
||||
{
|
||||
// 旋转轴状态变为 Error/Alarm 时记录日志 | Log when rotary axis status becomes Error/Alarm
|
||||
if (kvp.Value.Status == AxisStatus.Error || kvp.Value.Status == AxisStatus.Alarm)
|
||||
{
|
||||
_logger.Error(null, "旋转轴 {AxisId} 状态变为 {Status} | Rotary axis {AxisId} status changed to {Status}", kvp.Key, kvp.Value.Status);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 检测门状态变化 | Detect door status changes
|
||||
if (oldDoorStatus != _motionSystem.SafetyDoor.Status)
|
||||
{
|
||||
_eventAggregator.GetEvent<DoorStatusChangedEvent>().Publish(_motionSystem.SafetyDoor.Status);
|
||||
}
|
||||
|
||||
|
||||
// 检测联锁状态变化 | Detect interlock status changes
|
||||
if (oldInterlocked != _motionSystem.SafetyDoor.IsInterlocked)
|
||||
{
|
||||
_eventAggregator.GetEvent<DoorInterlockChangedEvent>()
|
||||
.Publish(_motionSystem.SafetyDoor.IsInterlocked);
|
||||
_logger.Info("门联锁状态变化：{IsInterlocked} | Door interlock status changed: {IsInterlocked}",
|
||||
_motionSystem.SafetyDoor.IsInterlocked);
|
||||
}
|
||||
// 轮询成功，重置错误计数 | Poll succeeded, reset error count
|
||||
_pollErrorCount = 0;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_pollErrorCount++;
|
||||
// 仅首次和每50次记录日志，避免刷屏 | Log only first and every 50th to avoid spam
|
||||
if (_pollErrorCount == 1 || _pollErrorCount % 50 == 0)
|
||||
_logger.Error(ex, "轮询异常（第{Count}次）| Polling error (#{Count}): {Message}", _pollErrorCount, _pollErrorCount, ex.Message);
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region 单轴移动 | Single Axis Move
|
||||
|
||||
/// <summary>
|
||||
/// 移动直线轴到目标位置 | Move linear axis to target position
|
||||
/// 包含边界检查和运动中防重入 | Includes boundary check and move-in-progress guard
|
||||
/// </summary>
|
||||
public MotionResult MoveToTarget(AxisId axisId, double target, double? speed = null)
|
||||
{
|
||||
var axis = _motionSystem.GetLinearAxis(axisId);
|
||||
|
||||
// 运动中防重入检查 | Move-in-progress guard
|
||||
if (axis.Status == AxisStatus.Moving)
|
||||
{
|
||||
_logger.Warn("直线轴 {AxisId} 正在运动中，拒绝移动命令 | Linear axis {AxisId} is moving, move command rejected", axisId);
|
||||
return MotionResult.Fail($"直线轴 {axisId} 正在运动中，拒绝重复命令 | Linear axis {axisId} is moving, duplicate command rejected");
|
||||
}
|
||||
|
||||
// 委托给轴实现（轴内部包含边界检查）| Delegate to axis implementation (axis contains boundary check)
|
||||
var result = axis.MoveToTarget(target, speed ?? _config.DefaultVelocity);
|
||||
|
||||
if (result.Success)
|
||||
{
|
||||
_logger.Info("直线轴 {AxisId} 移动命令已发送，目标位置={Target}mm | Linear axis {AxisId} move command sent, target={Target}mm", axisId, target);
|
||||
}
|
||||
else
|
||||
{
|
||||
_logger.Warn("直线轴 {AxisId} 移动命令被拒绝：{Reason} | Linear axis {AxisId} move command rejected: {Reason}", axisId, result.ErrorMessage);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 移动旋转轴到目标角度 | Move rotary axis to target angle
|
||||
/// 包含禁用轴检查和边界检查 | Includes disabled axis check and boundary check
|
||||
/// </summary>
|
||||
public MotionResult MoveRotaryToTarget(RotaryAxisId axisId, double targetAngle, double? speed = null)
|
||||
{
|
||||
var axis = _motionSystem.GetRotaryAxis(axisId);
|
||||
|
||||
// 禁用轴检查 | Disabled axis check
|
||||
if (!axis.Enabled)
|
||||
{
|
||||
_logger.Warn("旋转轴 {AxisId} 已禁用，拒绝移动命令 | Rotary axis {AxisId} is disabled, move command rejected", axisId);
|
||||
return MotionResult.Fail($"旋转轴 {axisId} 已禁用 | Rotary axis {axisId} is disabled");
|
||||
}
|
||||
|
||||
// 运动中防重入检查 | Move-in-progress guard
|
||||
if (axis.Status == AxisStatus.Moving)
|
||||
{
|
||||
_logger.Warn("旋转轴 {AxisId} 正在运动中，拒绝移动命令 | Rotary axis {AxisId} is moving, move command rejected", axisId);
|
||||
return MotionResult.Fail($"旋转轴 {axisId} 正在运动中，拒绝重复命令 | Rotary axis {axisId} is moving, duplicate command rejected");
|
||||
}
|
||||
|
||||
// 委托给轴实现（轴内部包含边界检查）| Delegate to axis implementation (axis contains boundary check)
|
||||
var result = axis.MoveToTarget(targetAngle, speed ?? _config.DefaultVelocity);
|
||||
|
||||
if (result.Success)
|
||||
{
|
||||
_logger.Info("旋转轴 {AxisId} 移动命令已发送，目标角度={TargetAngle}° | Rotary axis {AxisId} move command sent, target={TargetAngle}°", axisId, targetAngle);
|
||||
}
|
||||
else
|
||||
{
|
||||
_logger.Warn("旋转轴 {AxisId} 移动命令被拒绝：{Reason} | Rotary axis {AxisId} move command rejected: {Reason}", axisId, result.ErrorMessage);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region 多轴联动 | Multi-Axis Coordinated Move
|
||||
|
||||
/// <summary>
|
||||
/// 多轴联动移动 | Multi-axis coordinated move
|
||||
/// 原子性边界检查：先验证所有目标，任意轴越界则拒绝整个命令
|
||||
/// Atomic boundary check: validate all targets first, reject entire command if any axis out of range
|
||||
/// </summary>
|
||||
public MotionResult MoveAllToTarget(Dictionary<AxisId, double> targets)
|
||||
{
|
||||
if (targets == null || targets.Count == 0)
|
||||
{
|
||||
_logger.Warn("多轴联动目标为空，拒绝命令 | Multi-axis targets empty, command rejected");
|
||||
return MotionResult.Fail("多轴联动目标为空 | Multi-axis targets empty");
|
||||
}
|
||||
|
||||
// 原子性边界检查：收集所有错误 | Atomic boundary check: collect all errors
|
||||
var errors = new List<string>();
|
||||
|
||||
foreach (var kvp in targets)
|
||||
{
|
||||
var axis = _motionSystem.GetLinearAxis(kvp.Key);
|
||||
|
||||
// 运动中防重入检查 | Move-in-progress guard
|
||||
if (axis.Status == AxisStatus.Moving)
|
||||
{
|
||||
errors.Add($"直线轴 {kvp.Key} 正在运动中 | Linear axis {kvp.Key} is moving");
|
||||
continue;
|
||||
}
|
||||
|
||||
// 通过轴的 ValidateTarget 检查边界（需要转换为 LinearAxisBase）
|
||||
// 直接尝试调用 MoveToTarget 前先做边界预检查
|
||||
// 使用配置中的 Min/Max 进行边界检查 | Use config Min/Max for boundary check
|
||||
if (_config.LinearAxes.TryGetValue(kvp.Key, out var axisConfig))
|
||||
{
|
||||
if (kvp.Value < axisConfig.Min || kvp.Value > axisConfig.Max)
|
||||
{
|
||||
errors.Add($"直线轴 {kvp.Key} 目标位置 {kvp.Value} 超出范围 [{axisConfig.Min}, {axisConfig.Max}] | " +
|
||||
$"Linear axis {kvp.Key} target {kvp.Value} out of range [{axisConfig.Min}, {axisConfig.Max}]");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 如果有任何错误，拒绝整个命令 | If any errors, reject entire command
|
||||
if (errors.Count > 0)
|
||||
{
|
||||
var allErrors = string.Join("; ", errors);
|
||||
_logger.Warn("多轴联动边界检查失败：{Errors} | Multi-axis boundary check failed: {Errors}", allErrors);
|
||||
return MotionResult.Fail(allErrors);
|
||||
}
|
||||
|
||||
// 所有检查通过，同时向所有轴写入目标 | All checks passed, write targets to all axes simultaneously
|
||||
foreach (var kvp in targets)
|
||||
{
|
||||
var axis = _motionSystem.GetLinearAxis(kvp.Key);
|
||||
axis.MoveToTarget(kvp.Value);
|
||||
}
|
||||
|
||||
_logger.Info("多轴联动移动命令已发送，轴数={Count} | Multi-axis move command sent, axis count={Count}", targets.Count);
|
||||
return MotionResult.Ok();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 停止所有已启用轴 | Stop all enabled axes
|
||||
/// 包括所有直线轴和已启用的旋转轴 | Includes all linear axes and enabled rotary axes
|
||||
/// </summary>
|
||||
public MotionResult StopAll()
|
||||
{
|
||||
// 停止所有直线轴 | Stop all linear axes
|
||||
foreach (var kvp in _motionSystem.LinearAxes)
|
||||
{
|
||||
kvp.Value.Stop();
|
||||
}
|
||||
|
||||
// 停止所有已启用的旋转轴 | Stop all enabled rotary axes
|
||||
foreach (var kvp in _motionSystem.RotaryAxes)
|
||||
{
|
||||
if (kvp.Value.Enabled)
|
||||
{
|
||||
kvp.Value.Stop();
|
||||
}
|
||||
}
|
||||
|
||||
_logger.Info("全部停止命令已发送 | Stop all command sent");
|
||||
return MotionResult.Ok();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 所有已启用轴回零 | Home all enabled axes
|
||||
/// 包括所有直线轴和已启用的旋转轴 | Includes all linear axes and enabled rotary axes
|
||||
/// </summary>
|
||||
public MotionResult HomeAll()
|
||||
{
|
||||
// 回零所有直线轴 | Home all linear axes
|
||||
foreach (var kvp in _motionSystem.LinearAxes)
|
||||
{
|
||||
kvp.Value.Home();
|
||||
}
|
||||
|
||||
// 回零所有已启用的旋转轴 | Home all enabled rotary axes
|
||||
foreach (var kvp in _motionSystem.RotaryAxes)
|
||||
{
|
||||
if (kvp.Value.Enabled)
|
||||
{
|
||||
kvp.Value.Home();
|
||||
}
|
||||
}
|
||||
|
||||
_logger.Info("全部回零命令已发送 | Home all command sent");
|
||||
return MotionResult.Ok();
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Jog 控制 | Jog Control
|
||||
|
||||
/// <summary>
|
||||
/// 直线轴 Jog 启动 | Start linear axis jog
|
||||
/// Homing 状态下拒绝 Jog 命令 | Rejects jog when axis is homing
|
||||
/// </summary>
|
||||
public MotionResult JogStart(AxisId axisId, bool positive)
|
||||
{
|
||||
var axis = _motionSystem.GetLinearAxis(axisId);
|
||||
|
||||
// Homing 状态检查 | Homing status check
|
||||
if (axis.Status == AxisStatus.Homing)
|
||||
{
|
||||
_logger.Warn("直线轴 {AxisId} 正在回零，拒绝 Jog 命令 | Linear axis {AxisId} is homing, jog command rejected", axisId);
|
||||
return MotionResult.Fail($"直线轴 {axisId} 正在回零，拒绝 Jog 命令 | Linear axis {axisId} is homing, jog rejected");
|
||||
}
|
||||
|
||||
var result = axis.JogStart(positive);
|
||||
|
||||
if (result.Success)
|
||||
{
|
||||
var direction = positive ? "正向 | positive" : "反向 | negative";
|
||||
_logger.Info("直线轴 {AxisId} Jog 启动，方向={Direction} | Linear axis {AxisId} jog started, direction={Direction}", axisId, direction);
|
||||
}
|
||||
else
|
||||
{
|
||||
_logger.Warn("直线轴 {AxisId} Jog 启动被拒绝：{Reason} | Linear axis {AxisId} jog start rejected: {Reason}", axisId, result.ErrorMessage);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 直线轴 Jog 停止 | Stop linear axis jog
|
||||
/// </summary>
|
||||
public MotionResult JogStop(AxisId axisId)
|
||||
{
|
||||
var axis = _motionSystem.GetLinearAxis(axisId);
|
||||
var result = axis.JogStop();
|
||||
|
||||
if (result.Success)
|
||||
{
|
||||
_logger.Info("直线轴 {AxisId} Jog 已停止 | Linear axis {AxisId} jog stopped", axisId);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 旋转轴 Jog 启动 | Start rotary axis jog
|
||||
/// Homing 状态下拒绝 Jog 命令，禁用轴拒绝命令 | Rejects jog when homing or disabled
|
||||
/// </summary>
|
||||
public MotionResult JogRotaryStart(RotaryAxisId axisId, bool positive)
|
||||
{
|
||||
var axis = _motionSystem.GetRotaryAxis(axisId);
|
||||
|
||||
// 禁用轴检查 | Disabled axis check
|
||||
if (!axis.Enabled)
|
||||
{
|
||||
_logger.Warn("旋转轴 {AxisId} 已禁用，拒绝 Jog 命令 | Rotary axis {AxisId} is disabled, jog command rejected", axisId);
|
||||
return MotionResult.Fail($"旋转轴 {axisId} 已禁用 | Rotary axis {axisId} is disabled");
|
||||
}
|
||||
|
||||
// Homing 状态检查 | Homing status check
|
||||
if (axis.Status == AxisStatus.Homing)
|
||||
{
|
||||
_logger.Warn("旋转轴 {AxisId} 正在回零，拒绝 Jog 命令 | Rotary axis {AxisId} is homing, jog command rejected", axisId);
|
||||
return MotionResult.Fail($"旋转轴 {axisId} 正在回零，拒绝 Jog 命令 | Rotary axis {axisId} is homing, jog rejected");
|
||||
}
|
||||
|
||||
var result = axis.JogStart(positive);
|
||||
|
||||
if (result.Success)
|
||||
{
|
||||
var direction = positive ? "正向 | positive" : "反向 | negative";
|
||||
_logger.Info("旋转轴 {AxisId} Jog 启动，方向={Direction} | Rotary axis {AxisId} jog started, direction={Direction}", axisId, direction);
|
||||
}
|
||||
else
|
||||
{
|
||||
_logger.Warn("旋转轴 {AxisId} Jog 启动被拒绝：{Reason} | Rotary axis {AxisId} jog start rejected: {Reason}", axisId, result.ErrorMessage);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 旋转轴 Jog 停止 | Stop rotary axis jog
|
||||
/// </summary>
|
||||
public MotionResult JogRotaryStop(RotaryAxisId axisId)
|
||||
{
|
||||
var axis = _motionSystem.GetRotaryAxis(axisId);
|
||||
|
||||
// 禁用轴检查 | Disabled axis check
|
||||
if (!axis.Enabled)
|
||||
{
|
||||
_logger.Warn("旋转轴 {AxisId} 已禁用，拒绝 Jog 停止命令 | Rotary axis {AxisId} is disabled, jog stop rejected", axisId);
|
||||
return MotionResult.Fail($"旋转轴 {axisId} 已禁用 | Rotary axis {axisId} is disabled");
|
||||
}
|
||||
|
||||
var result = axis.JogStop();
|
||||
|
||||
if (result.Success)
|
||||
{
|
||||
_logger.Info("旋转轴 {AxisId} Jog 已停止 | Rotary axis {AxisId} jog stopped", axisId);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region 安全门控制 | Safety Door Control
|
||||
|
||||
/// <summary>
|
||||
/// 开门（含联锁检查）| Open door (with interlock check)
|
||||
/// 联锁信号有效时拒绝开门 | Rejects when interlock is active
|
||||
/// </summary>
|
||||
public MotionResult OpenDoor()
|
||||
{
|
||||
var door = _motionSystem.SafetyDoor;
|
||||
|
||||
// 检查联锁信号 | Check interlock signal
|
||||
if (door.IsInterlocked)
|
||||
{
|
||||
_logger.Warn("联锁信号有效，禁止开门 | Interlock active, door open blocked");
|
||||
return MotionResult.Fail("联锁信号有效，禁止开门 | Interlock active, door open blocked");
|
||||
}
|
||||
|
||||
// TODO: 预留射线源联锁扩展点 | Reserved extension point for ray source interlock
|
||||
// 当需要实现射线源联锁时，通过订阅 RaySourceStatusChangedEvent 缓存射线源状态
|
||||
// When implementing ray source interlock, subscribe to RaySourceStatusChangedEvent to cache ray source status
|
||||
// 在此处检查 _isRaySourceOn，若为 true 则拒绝开门
|
||||
// Check _isRaySourceOn here, reject door open if true
|
||||
// 示例代码 | Example code:
|
||||
// if (_isRaySourceOn)
|
||||
// {
|
||||
// _logger.Warn("射线源开启中，禁止开门 | Ray source is on, door open blocked");
|
||||
// return MotionResult.Fail("射线源开启中，禁止开门 | Ray source is on, door open blocked");
|
||||
// }
|
||||
|
||||
var result = door.Open();
|
||||
|
||||
if (result.Success)
|
||||
{
|
||||
_eventAggregator.GetEvent<DoorStatusChangedEvent>().Publish(DoorStatus.Opening);
|
||||
_logger.Info("开门命令已发送 | Open door command sent");
|
||||
}
|
||||
else
|
||||
{
|
||||
_logger.Warn("开门命令被拒绝：{Reason} | Open door command rejected: {Reason}", result.ErrorMessage);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 关门 | Close door
|
||||
/// </summary>
|
||||
public MotionResult CloseDoor()
|
||||
{
|
||||
var door = _motionSystem.SafetyDoor;
|
||||
var result = door.Close();
|
||||
|
||||
if (result.Success)
|
||||
{
|
||||
_eventAggregator.GetEvent<DoorStatusChangedEvent>().Publish(DoorStatus.Closing);
|
||||
_logger.Info("关门命令已发送 | Close door command sent");
|
||||
}
|
||||
else
|
||||
{
|
||||
_logger.Warn("关门命令被拒绝：{Reason} | Close door command rejected: {Reason}", result.ErrorMessage);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 停止门运动 | Stop door movement
|
||||
/// </summary>
|
||||
public MotionResult StopDoor()
|
||||
{
|
||||
var door = _motionSystem.SafetyDoor;
|
||||
var result = door.Stop();
|
||||
|
||||
if (result.Success)
|
||||
{
|
||||
_logger.Info("停止门命令已发送 | Stop door command sent");
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region 几何计算 | Geometry Calculation
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前几何参数（正算）| Get current geometry parameters (forward calculation)
|
||||
/// 根据当前轴位置和探测器摆动角度计算 FOD、FDD 和放大倍率
|
||||
/// Calculates FOD, FDD and magnification from current axis positions and detector swing angle
|
||||
/// </summary>
|
||||
public (double FOD, double FDD, double Magnification) GetCurrentGeometry()
|
||||
{
|
||||
// 获取 SourceZ 和 DetectorZ 的实际位置 | Get actual positions of SourceZ and DetectorZ
|
||||
var sourceZ = _motionSystem.GetLinearAxis(AxisId.SourceZ);
|
||||
var detectorZ = _motionSystem.GetLinearAxis(AxisId.DetectorZ);
|
||||
|
||||
// 计算绝对坐标 | Calculate absolute coordinates
|
||||
var sourceZAbsolute = _geometryCalculator.CalcAbsolutePosition(
|
||||
sourceZ.ActualPosition, _config.Geometry.SourceZOrigin);
|
||||
var detectorZAbsolute = _geometryCalculator.CalcAbsolutePosition(
|
||||
detectorZ.ActualPosition, _config.Geometry.DetectorZOrigin);
|
||||
|
||||
// 获取探测器摆动角度 | Get detector swing angle
|
||||
var detectorSwing = _motionSystem.GetRotaryAxis(RotaryAxisId.DetectorSwing);
|
||||
double swingAngle = detectorSwing.ActualAngle;
|
||||
|
||||
// 计算 FDD（考虑摆动）| Calculate FDD with swing
|
||||
var fdd = _geometryCalculator.CalcFDD(
|
||||
sourceZAbsolute, detectorZAbsolute,
|
||||
_config.Geometry.SwingPivotOffset, swingAngle, _config.Geometry.SwingRadius);
|
||||
|
||||
// 计算 FOD（考虑摆动）| Calculate FOD with swing
|
||||
var fod = _geometryCalculator.CalcFOD(
|
||||
sourceZAbsolute, _config.Geometry.StageRotationCenterZ,
|
||||
detectorZAbsolute, _config.Geometry.SwingPivotOffset,
|
||||
swingAngle, _config.Geometry.SwingRadius);
|
||||
|
||||
// 计算放大倍率 | Calculate magnification
|
||||
var magnification = _geometryCalculator.CalcMagnification(fdd, fod);
|
||||
|
||||
return (fod, fdd, magnification);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 应用几何参数（反算，由 FOD 和 FDD）| Apply geometry (inverse, from FOD and FDD)
|
||||
/// 根据当前探测器摆动角度，计算并移动 SourceZ 和 DetectorZ 到目标位置
|
||||
/// Calculates and moves SourceZ and DetectorZ to target positions based on current detector swing angle
|
||||
/// </summary>
|
||||
public MotionResult ApplyGeometry(double targetFOD, double targetFDD)
|
||||
{
|
||||
// 获取当前探测器摆动角度 | Get current detector swing angle
|
||||
var detectorSwing = _motionSystem.GetRotaryAxis(RotaryAxisId.DetectorSwing);
|
||||
double swingAngle = detectorSwing.ActualAngle;
|
||||
|
||||
// 反算轴目标位置（考虑摆动）| Inverse calculate axis target positions with swing
|
||||
var (sourceZTarget, detectorZTarget, errorMessage) = _geometryCalculator.CalcAxisTargets(
|
||||
targetFOD, targetFDD,
|
||||
_config.Geometry.StageRotationCenterZ,
|
||||
_config.Geometry.SourceZOrigin,
|
||||
_config.Geometry.DetectorZOrigin,
|
||||
_config.Geometry.SwingPivotOffset,
|
||||
swingAngle,
|
||||
_config.Geometry.SwingRadius);
|
||||
|
||||
// 反算失败（如目标不可达）| Inverse calculation failed (e.g. target unreachable)
|
||||
if (errorMessage != null)
|
||||
{
|
||||
_logger.Warn("几何反算失败：{Reason} | Geometry inverse failed: {Reason}", errorMessage);
|
||||
MessageBox.Show(
|
||||
LocalizationHelper.Get("MC_GeometryCalcFailed", errorMessage),
|
||||
LocalizationHelper.Get("MC_Geometry_Title"),
|
||||
MessageBoxButton.OK, MessageBoxImage.Warning);
|
||||
return MotionResult.Fail(errorMessage);
|
||||
}
|
||||
|
||||
// 获取 SourceZ 和 DetectorZ 的配置范围 | Get SourceZ and DetectorZ config ranges
|
||||
var sourceZConfig = _config.LinearAxes[AxisId.SourceZ];
|
||||
var detectorZConfig = _config.LinearAxes[AxisId.DetectorZ];
|
||||
|
||||
// 边界检查 | Boundary validation
|
||||
var validateResult = _geometryCalculator.ValidateTargets(
|
||||
sourceZTarget, detectorZTarget,
|
||||
sourceZConfig.Min, sourceZConfig.Max,
|
||||
detectorZConfig.Min, detectorZConfig.Max);
|
||||
|
||||
if (!validateResult.Success)
|
||||
{
|
||||
_logger.Warn("几何反算边界检查失败：{Reason} | Geometry inverse boundary check failed: {Reason}", validateResult.ErrorMessage);
|
||||
MessageBox.Show(
|
||||
LocalizationHelper.Get("MC_GeometryCalcFailed", validateResult.ErrorMessage),
|
||||
LocalizationHelper.Get("MC_Geometry_Title"),
|
||||
MessageBoxButton.OK, MessageBoxImage.Warning);
|
||||
return validateResult;
|
||||
}
|
||||
|
||||
// 弹出确认窗口，显示计算结果 | Show confirmation dialog with calculation results
|
||||
var confirmMessage = LocalizationHelper.Get("MC_GeometryConfirm",
|
||||
targetFOD, targetFDD, swingAngle, sourceZTarget, detectorZTarget);
|
||||
var confirmResult = MessageBox.Show(
|
||||
confirmMessage,
|
||||
LocalizationHelper.Get("MC_Geometry_Title"),
|
||||
MessageBoxButton.OKCancel, MessageBoxImage.Question);
|
||||
|
||||
if (confirmResult != MessageBoxResult.OK)
|
||||
{
|
||||
_logger.Info("用户取消几何反算移动 | User cancelled geometry inverse move");
|
||||
return MotionResult.Fail(LocalizationHelper.Get("MC_GeometryCancelled"));
|
||||
}
|
||||
|
||||
// 同时移动 SourceZ 和 DetectorZ（多轴联动）| Move SourceZ and DetectorZ simultaneously (coordinated move)
|
||||
var targets = new Dictionary<AxisId, double>
|
||||
{
|
||||
{ AxisId.SourceZ, sourceZTarget },
|
||||
{ AxisId.DetectorZ, detectorZTarget }
|
||||
};
|
||||
|
||||
var moveResult = MoveAllToTarget(targets);
|
||||
|
||||
if (moveResult.Success)
|
||||
{
|
||||
_logger.Info("几何反算移动命令已发送，目标 FOD={TargetFOD}mm, FDD={TargetFDD}mm, 摆动角度={SwingAngle}° | " +
|
||||
"Geometry inverse move sent, target FOD={TargetFOD}mm, FDD={TargetFDD}mm, swing={SwingAngle}°",
|
||||
targetFOD, targetFDD, swingAngle);
|
||||
}
|
||||
|
||||
return moveResult;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 应用几何参数（反算，由 FOD 和放大倍率）| Apply geometry (inverse, from FOD and magnification)
|
||||
/// 先计算 FDD = M × FOD，再移动轴 | Calculates FDD = M × FOD, then moves axes
|
||||
/// </summary>
|
||||
public MotionResult ApplyGeometryByMagnification(double targetFOD, double magnification)
|
||||
{
|
||||
// 计算 FDD = M × FOD | Calculate FDD = M × FOD
|
||||
var targetFDD = magnification * targetFOD;
|
||||
|
||||
_logger.Info("由放大倍率计算 FDD：M={Magnification}, FOD={FOD}mm, FDD={FDD}mm | Calculate FDD from magnification: M={Magnification}, FOD={FOD}mm, FDD={FDD}mm",
|
||||
magnification, targetFOD, targetFDD);
|
||||
|
||||
return ApplyGeometry(targetFOD, targetFDD);
|
||||
}
|
||||
|
||||
|
||||
/// <inheritdoc/>
|
||||
public (double SourceZTarget, double DetectorZTarget, string ErrorMessage) CalculateGeometryTargets(double targetFOD, double targetFDD)
|
||||
{
|
||||
// 获取当前探测器摆动角度 | Get current detector swing angle
|
||||
var detectorSwing = _motionSystem.GetRotaryAxis(RotaryAxisId.DetectorSwing);
|
||||
double swingAngle = detectorSwing.ActualAngle;
|
||||
|
||||
// 反算轴目标位置（考虑摆动）| Inverse calculate axis target positions with swing
|
||||
var (sourceZTarget, detectorZTarget, errorMessage) = _geometryCalculator.CalcAxisTargets(
|
||||
targetFOD, targetFDD,
|
||||
_config.Geometry.StageRotationCenterZ,
|
||||
_config.Geometry.SourceZOrigin,
|
||||
_config.Geometry.DetectorZOrigin,
|
||||
_config.Geometry.SwingPivotOffset,
|
||||
swingAngle,
|
||||
_config.Geometry.SwingRadius);
|
||||
|
||||
if (errorMessage != null)
|
||||
{
|
||||
_logger.Warn("几何反算失败：{Reason} | Geometry inverse failed: {Reason}", errorMessage);
|
||||
MessageBox.Show(
|
||||
LocalizationHelper.Get("MC_GeometryCalcFailed", errorMessage),
|
||||
LocalizationHelper.Get("MC_Geometry_Title"),
|
||||
MessageBoxButton.OK, MessageBoxImage.Warning);
|
||||
return (0, 0, errorMessage);
|
||||
}
|
||||
|
||||
// 边界检查 | Boundary validation
|
||||
var sourceZConfig = _config.LinearAxes[AxisId.SourceZ];
|
||||
var detectorZConfig = _config.LinearAxes[AxisId.DetectorZ];
|
||||
var validateResult = _geometryCalculator.ValidateTargets(
|
||||
sourceZTarget, detectorZTarget,
|
||||
sourceZConfig.Min, sourceZConfig.Max,
|
||||
detectorZConfig.Min, detectorZConfig.Max);
|
||||
|
||||
if (!validateResult.Success)
|
||||
{
|
||||
_logger.Warn("几何反算边界检查失败：{Reason} | Geometry inverse boundary check failed: {Reason}", validateResult.ErrorMessage);
|
||||
MessageBox.Show(
|
||||
LocalizationHelper.Get("MC_GeometryCalcFailed", validateResult.ErrorMessage),
|
||||
LocalizationHelper.Get("MC_Geometry_Title"),
|
||||
MessageBoxButton.OK, MessageBoxImage.Warning);
|
||||
return (0, 0, validateResult.ErrorMessage);
|
||||
}
|
||||
|
||||
_logger.Info("几何反算计算完成，SourceZ={SourceZ}, DetectorZ={DetectorZ} | Geometry inverse calculated, SourceZ={SourceZ}, DetectorZ={DetectorZ}",
|
||||
sourceZTarget, detectorZTarget);
|
||||
return (sourceZTarget, detectorZTarget, null);
|
||||
}
|
||||
|
||||
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user