将Feature/XP.Common和Feature/XP.Hardware分支合并至Develop/XP.forHardwareAndCommon，完善XPapp注册和相关硬件类库通用类库功能。
This commit is contained in:
QI Mingxuan
@@ -0,0 +1,303 @@
|
||||
using System;
|
||||
using XP.Hardware.MotionControl.Abstractions;
|
||||
|
||||
namespace XP.Hardware.MotionControl.Services
|
||||
{
|
||||
/// <summary>
|
||||
/// 几何计算器 | Geometry Calculator
|
||||
/// 负责 FOD/FDD/放大倍率的正算与反算，支持探测器摆动角度 | Forward and inverse calculation of FOD/FDD/Magnification with detector swing support
|
||||
/// </summary>
|
||||
public class GeometryCalculator
|
||||
{
|
||||
/// <summary>
|
||||
/// FOD 接近零的阈值（mm）| Threshold for FOD near zero (mm)
|
||||
/// </summary>
|
||||
private const double FodZeroThreshold = 0.001;
|
||||
|
||||
/// <summary>
|
||||
/// 角度接近零的阈值（度）| Threshold for angle near zero (degrees)
|
||||
/// </summary>
|
||||
private const double AngleZeroThreshold = 0.001;
|
||||
|
||||
#region 绝对坐标计算 | Absolute Coordinate Calculation
|
||||
|
||||
/// <summary>
|
||||
/// 计算绝对坐标 | Calculate absolute coordinate
|
||||
/// 绝对坐标 = 当前位置 + Origin（配置）| Absolute = current position + Origin (config)
|
||||
/// </summary>
|
||||
/// <param name="currentPosition">当前位置（mm）| Current position (mm)</param>
|
||||
/// <param name="origin">原点偏移（mm）| Origin offset (mm)</param>
|
||||
/// <returns>绝对坐标（mm）| Absolute coordinate (mm)</returns>
|
||||
public double CalcAbsolutePosition(double currentPosition, double origin)
|
||||
{
|
||||
return currentPosition + origin;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region 探测器中心坐标计算 | Detector Center Coordinate Calculation
|
||||
|
||||
/// <summary>
|
||||
/// 计算探测器感光面中心的 X 坐标 | Calculate detector active area center X coordinate
|
||||
/// Dx = R × sin(θ)
|
||||
/// </summary>
|
||||
/// <param name="swingAngleDeg">探测器摆动角度（度）| Detector swing angle (degrees)</param>
|
||||
/// <param name="swingRadius">摆动半径（mm）| Swing radius (mm)</param>
|
||||
/// <returns>探测器中心 X 坐标（mm）| Detector center X (mm)</returns>
|
||||
public double CalcDetectorCenterX(double swingAngleDeg, double swingRadius)
|
||||
{
|
||||
if (swingRadius <= 0 || Math.Abs(swingAngleDeg) < AngleZeroThreshold)
|
||||
return 0.0;
|
||||
|
||||
double thetaRad = swingAngleDeg * Math.PI / 180.0;
|
||||
return swingRadius * Math.Sin(thetaRad);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算探测器感光面中心的 Z 坐标 | Calculate detector active area center Z coordinate
|
||||
/// Dz = Pz - R × cos(θ)，其中 Pz = detectorZAbsolute + swingPivotOffset
|
||||
/// 探测器感光面在 Pivot 下方（朝向射线源）| Detector active area is below Pivot (toward source)
|
||||
/// </summary>
|
||||
/// <param name="detectorZAbsolute">探测器Z轴绝对坐标（mm）| Detector Z absolute (mm)</param>
|
||||
/// <param name="swingPivotOffset">Pivot 相对于 DetectorZ 绝对坐标的偏移（mm）| Pivot offset from DetectorZ absolute (mm)</param>
|
||||
/// <param name="swingAngleDeg">探测器摆动角度（度）| Detector swing angle (degrees)</param>
|
||||
/// <param name="swingRadius">摆动半径（mm）| Swing radius (mm)</param>
|
||||
/// <returns>探测器中心 Z 坐标（mm）| Detector center Z (mm)</returns>
|
||||
public double CalcDetectorCenterZ(double detectorZAbsolute, double swingPivotOffset,
|
||||
double swingAngleDeg, double swingRadius)
|
||||
{
|
||||
double pivotZ = detectorZAbsolute + swingPivotOffset;
|
||||
|
||||
if (swingRadius <= 0)
|
||||
return detectorZAbsolute;
|
||||
|
||||
double thetaRad = swingAngleDeg * Math.PI / 180.0;
|
||||
return pivotZ - swingRadius * Math.Cos(thetaRad);
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region 正算 | Forward Calculation
|
||||
|
||||
/// <summary>
|
||||
/// 正算：计算 FDD（考虑探测器摆动）| Forward: Calculate FDD with detector swing
|
||||
/// FDD = sqrt(Dx² + (Dz - Sz)²)
|
||||
/// 当 swingRadius = 0 时退化为 |Sz - DetectorZ_abs| | Degrades to |Sz - DetectorZ_abs| when swingRadius = 0
|
||||
/// </summary>
|
||||
/// <param name="sourceZAbsolute">射线源Z轴绝对坐标（mm）| Source Z absolute (mm)</param>
|
||||
/// <param name="detectorZAbsolute">探测器Z轴绝对坐标（mm）| Detector Z absolute (mm)</param>
|
||||
/// <param name="swingPivotOffset">Pivot 偏移（mm）| Pivot offset (mm)</param>
|
||||
/// <param name="swingAngleDeg">摆动角度（度）| Swing angle (degrees)</param>
|
||||
/// <param name="swingRadius">摆动半径（mm）| Swing radius (mm)</param>
|
||||
/// <returns>FDD 值（mm）| FDD value (mm)</returns>
|
||||
public double CalcFDD(double sourceZAbsolute, double detectorZAbsolute,
|
||||
double swingPivotOffset, double swingAngleDeg, double swingRadius)
|
||||
{
|
||||
// 无摆动时退化为原始公式 | Degrade to original formula when no swing
|
||||
if (swingRadius <= 0 || Math.Abs(swingAngleDeg) < AngleZeroThreshold)
|
||||
return Math.Abs(sourceZAbsolute - detectorZAbsolute);
|
||||
|
||||
double dx = CalcDetectorCenterX(swingAngleDeg, swingRadius);
|
||||
double dz = CalcDetectorCenterZ(detectorZAbsolute, swingPivotOffset, swingAngleDeg, swingRadius);
|
||||
|
||||
return Math.Sqrt(dx * dx + (dz - sourceZAbsolute) * (dz - sourceZAbsolute));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 正算：计算 FOD（考虑探测器摆动）| Forward: Calculate FOD with detector swing
|
||||
/// FOD = 射线源到射线束与载物台平面交点的距离 | Distance from source to ray-beam/stage-plane intersection
|
||||
/// 当 swingRadius = 0 时退化为 |Sz - StageRotationCenterZ| | Degrades when swingRadius = 0
|
||||
/// </summary>
|
||||
/// <param name="sourceZAbsolute">射线源Z轴绝对坐标（mm）| Source Z absolute (mm)</param>
|
||||
/// <param name="stageRotationCenterZ">旋转中心Z坐标（mm）| Stage rotation center Z (mm)</param>
|
||||
/// <param name="detectorZAbsolute">探测器Z轴绝对坐标（mm）| Detector Z absolute (mm)</param>
|
||||
/// <param name="swingPivotOffset">Pivot 偏移（mm）| Pivot offset (mm)</param>
|
||||
/// <param name="swingAngleDeg">摆动角度（度）| Swing angle (degrees)</param>
|
||||
/// <param name="swingRadius">摆动半径（mm）| Swing radius (mm)</param>
|
||||
/// <returns>FOD 值（mm）| FOD value (mm)</returns>
|
||||
public double CalcFOD(double sourceZAbsolute, double stageRotationCenterZ,
|
||||
double detectorZAbsolute, double swingPivotOffset,
|
||||
double swingAngleDeg, double swingRadius)
|
||||
{
|
||||
// 无摆动时退化为原始公式 | Degrade to original formula when no swing
|
||||
if (swingRadius <= 0 || Math.Abs(swingAngleDeg) < AngleZeroThreshold)
|
||||
return Math.Abs(sourceZAbsolute - stageRotationCenterZ);
|
||||
|
||||
double dx = CalcDetectorCenterX(swingAngleDeg, swingRadius);
|
||||
double dz = CalcDetectorCenterZ(detectorZAbsolute, swingPivotOffset, swingAngleDeg, swingRadius);
|
||||
|
||||
double deltaZ_SD = dz - sourceZAbsolute;
|
||||
|
||||
// 射线源和探测器中心在同一 Z 高度时，射线束水平，无法与载物台平面相交 | Horizontal beam, no intersection
|
||||
if (Math.Abs(deltaZ_SD) < FodZeroThreshold)
|
||||
return double.NaN;
|
||||
|
||||
// 射线束与载物台平面交点参数 t_stage = (Oz - Sz) / (Dz - Sz) | Ray-plane intersection parameter
|
||||
double tStage = (stageRotationCenterZ - sourceZAbsolute) / deltaZ_SD;
|
||||
|
||||
// 交点 X 坐标 | Intersection X coordinate
|
||||
double xIntersect = tStage * dx;
|
||||
|
||||
// FOD = sqrt(x_intersect² + (Oz - Sz)²) | Euclidean distance from source to intersection
|
||||
double deltaZ_SO = stageRotationCenterZ - sourceZAbsolute;
|
||||
return Math.Sqrt(xIntersect * xIntersect + deltaZ_SO * deltaZ_SO);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 正算：计算放大倍率 | Forward: Calculate Magnification
|
||||
/// M = FDD / FOD，FOD 接近零（< 0.001mm）或 NaN 时返回 double.NaN
|
||||
/// </summary>
|
||||
/// <param name="fdd">焦点到探测器距离（mm）| Focus-to-detector distance (mm)</param>
|
||||
/// <param name="fod">焦点到旋转中心距离（mm）| Focus-to-object distance (mm)</param>
|
||||
/// <returns>放大倍率，FOD 接近零时返回 NaN | Magnification, NaN when FOD near zero</returns>
|
||||
public double CalcMagnification(double fdd, double fod)
|
||||
{
|
||||
if (double.IsNaN(fod) || fod < FodZeroThreshold)
|
||||
return double.NaN;
|
||||
|
||||
return fdd / fod;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region 反算 | Inverse Calculation
|
||||
|
||||
/// <summary>
|
||||
/// 反算：由目标 FOD 和 FDD 计算轴目标位置（考虑探测器摆动）| Inverse: Calculate axis targets from FOD and FDD with swing
|
||||
/// 返回值为相对坐标（已减去 Origin）| Returns relative coordinates (Origin subtracted)
|
||||
/// </summary>
|
||||
/// <param name="targetFOD">目标 FOD（mm）| Target FOD (mm)</param>
|
||||
/// <param name="targetFDD">目标 FDD（mm）| Target FDD (mm)</param>
|
||||
/// <param name="stageRotationCenterZ">旋转中心绝对Z坐标（mm）| Stage rotation center Z (mm)</param>
|
||||
/// <param name="sourceZOrigin">射线源Z轴原点偏移（mm）| Source Z origin offset (mm)</param>
|
||||
/// <param name="detectorZOrigin">探测器Z轴原点偏移（mm）| Detector Z origin offset (mm)</param>
|
||||
/// <param name="swingPivotOffset">Pivot 偏移（mm）| Pivot offset (mm)</param>
|
||||
/// <param name="swingAngleDeg">摆动角度（度）| Swing angle (degrees)</param>
|
||||
/// <param name="swingRadius">摆动半径（mm）| Swing radius (mm)</param>
|
||||
/// <returns>
|
||||
/// sourceZTarget: 射线源Z轴相对目标位置（mm）| Source Z relative target (mm)
|
||||
/// detectorZTarget: 探测器Z轴相对目标位置（mm）| Detector Z relative target (mm)
|
||||
/// errorMessage: 错误信息，成功时为 null | Error message, null on success
|
||||
/// </returns>
|
||||
public (double sourceZTarget, double detectorZTarget, string errorMessage) CalcAxisTargets(
|
||||
double targetFOD, double targetFDD,
|
||||
double stageRotationCenterZ, double sourceZOrigin, double detectorZOrigin,
|
||||
double swingPivotOffset, double swingAngleDeg, double swingRadius)
|
||||
{
|
||||
// 无摆动时使用简化公式 | Use simplified formula when no swing
|
||||
if (swingRadius <= 0 || Math.Abs(swingAngleDeg) < AngleZeroThreshold)
|
||||
{
|
||||
double szAbs = stageRotationCenterZ - targetFOD;
|
||||
double dzAbs = szAbs + targetFDD;
|
||||
return (szAbs - sourceZOrigin, dzAbs - detectorZOrigin, null);
|
||||
}
|
||||
|
||||
double thetaRad = swingAngleDeg * Math.PI / 180.0;
|
||||
double sinTheta = Math.Sin(thetaRad);
|
||||
double cosTheta = Math.Cos(thetaRad);
|
||||
|
||||
// B = R × sin(θ)，探测器中心的 X 偏移 | Detector center X offset
|
||||
double bx = swingRadius * sinTheta;
|
||||
// A = SwingPivotOffset - R × cos(θ)，DetectorZ_abs 到探测器中心的 Z 偏移 | DetectorZ_abs to detector center Z offset
|
||||
double az = swingPivotOffset - swingRadius * cosTheta;
|
||||
|
||||
// 由 targetFOD 求 SourceZ 绝对坐标 | Calculate SourceZ absolute from targetFOD
|
||||
// 射线源在载物台下方：Sz = Oz - targetFOD | Source below stage
|
||||
double sourceZAbsolute = stageRotationCenterZ - targetFOD;
|
||||
|
||||
// 由 targetFDD 求 DetectorZ 绝对坐标 | Calculate DetectorZ absolute from targetFDD
|
||||
// FDD² = Bx² + (DetZ_abs + Az - Sz)²
|
||||
// (DetZ_abs + Az - Sz)² = FDD² - Bx²
|
||||
double fddSquared = targetFDD * targetFDD;
|
||||
double bxSquared = bx * bx;
|
||||
double discriminant = fddSquared - bxSquared;
|
||||
|
||||
if (discriminant < 0)
|
||||
{
|
||||
return (0, 0,
|
||||
$"目标 FDD={targetFDD:F3}mm 小于探测器 X 偏移={Math.Abs(bx):F3}mm，目标不可达 | " +
|
||||
$"Target FDD={targetFDD:F3}mm less than detector X offset={Math.Abs(bx):F3}mm, target unreachable");
|
||||
}
|
||||
|
||||
// DetZ_abs = Sz - Az + sqrt(FDD² - Bx²)（探测器在射线源上方，取正值）
|
||||
// DetZ_abs = Sz - Az + sqrt(discriminant)
|
||||
double detectorZAbsolute = sourceZAbsolute - az + Math.Sqrt(discriminant);
|
||||
|
||||
// 转换为相对坐标 | Convert to relative coordinates
|
||||
double sourceZTarget = sourceZAbsolute - sourceZOrigin;
|
||||
double detectorZTarget = detectorZAbsolute - detectorZOrigin;
|
||||
|
||||
return (sourceZTarget, detectorZTarget, null);
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region 边界检查 | Boundary Validation
|
||||
|
||||
/// <summary>
|
||||
/// 边界检查：验证目标位置是否在允许范围内 | Boundary validation: Check targets within allowed range
|
||||
/// </summary>
|
||||
/// <param name="sourceZTarget">射线源Z轴目标位置（mm）| Source Z target (mm)</param>
|
||||
/// <param name="detectorZTarget">探测器Z轴目标位置（mm）| Detector Z target (mm)</param>
|
||||
/// <param name="sourceZMin">射线源Z轴最小值（mm）| Source Z minimum (mm)</param>
|
||||
/// <param name="sourceZMax">射线源Z轴最大值（mm）| Source Z maximum (mm)</param>
|
||||
/// <param name="detectorZMin">探测器Z轴最小值（mm）| Detector Z minimum (mm)</param>
|
||||
/// <param name="detectorZMax">探测器Z轴最大值（mm）| Detector Z maximum (mm)</param>
|
||||
/// <returns>验证结果 | Validation result</returns>
|
||||
public MotionResult ValidateTargets(
|
||||
double sourceZTarget, double detectorZTarget,
|
||||
double sourceZMin, double sourceZMax,
|
||||
double detectorZMin, double detectorZMax)
|
||||
{
|
||||
if (sourceZTarget < sourceZMin || sourceZTarget > sourceZMax)
|
||||
{
|
||||
return MotionResult.Fail(
|
||||
$"SourceZ 目标位置 {sourceZTarget:F3} 超出范围 [{sourceZMin:F3}, {sourceZMax:F3}] | " +
|
||||
$"SourceZ target {sourceZTarget:F3} out of range [{sourceZMin:F3}, {sourceZMax:F3}]");
|
||||
}
|
||||
|
||||
if (detectorZTarget < detectorZMin || detectorZTarget > detectorZMax)
|
||||
{
|
||||
return MotionResult.Fail(
|
||||
$"DetectorZ 目标位置 {detectorZTarget:F3} 超出范围 [{detectorZMin:F3}, {detectorZMax:F3}] | " +
|
||||
$"DetectorZ target {detectorZTarget:F3} out of range [{detectorZMin:F3}, {detectorZMax:F3}]");
|
||||
}
|
||||
|
||||
return MotionResult.Ok();
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region 向后兼容的重载 | Backward-Compatible Overloads
|
||||
|
||||
/// <summary>
|
||||
/// 正算 FOD（无摆动，向后兼容）| Forward FOD (no swing, backward compatible)
|
||||
/// </summary>
|
||||
public double CalcFOD(double sourceZAbsolute, double stageRotationCenterZ)
|
||||
{
|
||||
return Math.Abs(sourceZAbsolute - stageRotationCenterZ);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 正算 FDD（无摆动，向后兼容）| Forward FDD (no swing, backward compatible)
|
||||
/// </summary>
|
||||
public double CalcFDD(double sourceZAbsolute, double detectorZAbsolute)
|
||||
{
|
||||
return Math.Abs(sourceZAbsolute - detectorZAbsolute);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 反算轴目标（无摆动，向后兼容）| Inverse axis targets (no swing, backward compatible)
|
||||
/// </summary>
|
||||
public (double sourceZTarget, double detectorZTarget) CalcAxisTargetsSimple(
|
||||
double targetFOD, double targetFDD,
|
||||
double stageRotationCenterZ, double sourceZOrigin, double detectorZOrigin)
|
||||
{
|
||||
double sourceZAbsolute = stageRotationCenterZ - targetFOD;
|
||||
double detectorZAbsolute = sourceZAbsolute + targetFDD;
|
||||
return (sourceZAbsolute - sourceZOrigin, detectorZAbsolute - detectorZOrigin);
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,172 @@
|
||||
using System.Collections.Generic;
|
||||
using XP.Hardware.MotionControl.Abstractions;
|
||||
using XP.Hardware.MotionControl.Abstractions.Enums;
|
||||
|
||||
namespace XP.Hardware.MotionControl.Services
|
||||
{
|
||||
/// <summary>
|
||||
/// 运动控制业务服务接口 | Motion Control Business Service Interface
|
||||
/// 封装所有运动控制业务规则，供 ViewModel 调用 | Encapsulates all motion control business rules for ViewModel
|
||||
/// </summary>
|
||||
public interface IMotionControlService
|
||||
{
|
||||
#region 轮询控制 | Polling Control
|
||||
|
||||
/// <summary>
|
||||
/// 启动 PLC 状态轮询 | Start PLC status polling
|
||||
/// 以配置的 PollingInterval 周期执行轮询 | Polls at configured PollingInterval
|
||||
/// </summary>
|
||||
void StartPolling();
|
||||
|
||||
/// <summary>
|
||||
/// 停止 PLC 状态轮询 | Stop PLC status polling
|
||||
/// </summary>
|
||||
void StopPolling();
|
||||
|
||||
#endregion
|
||||
|
||||
#region 单轴移动 | Single Axis Move
|
||||
|
||||
/// <summary>
|
||||
/// 移动直线轴到目标位置 | Move linear axis to target position
|
||||
/// 包含边界检查和运动中防重入 | Includes boundary check and move-in-progress guard
|
||||
/// </summary>
|
||||
/// <param name="axisId">直线轴标识 | Linear axis identifier</param>
|
||||
/// <param name="target">目标位置（mm）| Target position (mm)</param>
|
||||
/// <returns>操作结果 | Operation result</returns>
|
||||
MotionResult MoveToTarget(AxisId axisId, double target, double? speed = null);
|
||||
|
||||
/// <summary>
|
||||
/// 移动旋转轴到目标角度 | Move rotary axis to target angle
|
||||
/// 包含边界检查和禁用轴检查 | Includes boundary check and disabled axis check
|
||||
/// </summary>
|
||||
/// <param name="axisId">旋转轴标识 | Rotary axis identifier</param>
|
||||
/// <param name="targetAngle">目标角度（度）| Target angle (degrees)</param>
|
||||
/// <param name="speed">运动速度（可选，不传则不写入速度信号）| Speed (optional)</param>
|
||||
/// <returns>操作结果 | Operation result</returns>
|
||||
MotionResult MoveRotaryToTarget(RotaryAxisId axisId, double targetAngle, double? speed = null);
|
||||
|
||||
#endregion
|
||||
|
||||
#region 多轴联动 | Multi-Axis Coordinated Move
|
||||
|
||||
/// <summary>
|
||||
/// 多轴联动移动 | Multi-axis coordinated move
|
||||
/// 对所有目标轴并行执行边界检查，任意轴越界则拒绝整个命令 | Atomic boundary check for all target axes
|
||||
/// </summary>
|
||||
/// <param name="targets">轴标识与目标位置的字典 | Dictionary of axis IDs and target positions</param>
|
||||
/// <returns>操作结果 | Operation result</returns>
|
||||
MotionResult MoveAllToTarget(Dictionary<AxisId, double> targets);
|
||||
|
||||
/// <summary>
|
||||
/// 停止所有已启用轴 | Stop all enabled axes
|
||||
/// </summary>
|
||||
/// <returns>操作结果 | Operation result</returns>
|
||||
MotionResult StopAll();
|
||||
|
||||
/// <summary>
|
||||
/// 所有已启用轴回零 | Home all enabled axes
|
||||
/// </summary>
|
||||
/// <returns>操作结果 | Operation result</returns>
|
||||
MotionResult HomeAll();
|
||||
|
||||
#endregion
|
||||
|
||||
#region Jog 控制 | Jog Control
|
||||
|
||||
/// <summary>
|
||||
/// 直线轴 Jog 启动 | Start linear axis jog
|
||||
/// Homing 状态下拒绝 Jog 命令 | Rejects jog when axis is homing
|
||||
/// </summary>
|
||||
/// <param name="axisId">直线轴标识 | Linear axis identifier</param>
|
||||
/// <param name="positive">正向为 true，反向为 false | True for positive, false for negative</param>
|
||||
/// <returns>操作结果 | Operation result</returns>
|
||||
MotionResult JogStart(AxisId axisId, bool positive);
|
||||
|
||||
/// <summary>
|
||||
/// 直线轴 Jog 停止 | Stop linear axis jog
|
||||
/// </summary>
|
||||
/// <param name="axisId">直线轴标识 | Linear axis identifier</param>
|
||||
/// <returns>操作结果 | Operation result</returns>
|
||||
MotionResult JogStop(AxisId axisId);
|
||||
|
||||
/// <summary>
|
||||
/// 旋转轴 Jog 启动 | Start rotary axis jog
|
||||
/// Homing 状态下拒绝 Jog 命令 | Rejects jog when axis is homing
|
||||
/// </summary>
|
||||
/// <param name="axisId">旋转轴标识 | Rotary axis identifier</param>
|
||||
/// <param name="positive">正向为 true，反向为 false | True for positive, false for negative</param>
|
||||
/// <returns>操作结果 | Operation result</returns>
|
||||
MotionResult JogRotaryStart(RotaryAxisId axisId, bool positive);
|
||||
|
||||
/// <summary>
|
||||
/// 旋转轴 Jog 停止 | Stop rotary axis jog
|
||||
/// </summary>
|
||||
/// <param name="axisId">旋转轴标识 | Rotary axis identifier</param>
|
||||
/// <returns>操作结果 | Operation result</returns>
|
||||
MotionResult JogRotaryStop(RotaryAxisId axisId);
|
||||
|
||||
#endregion
|
||||
|
||||
#region 安全门控制 | Safety Door Control
|
||||
|
||||
/// <summary>
|
||||
/// 开门（含联锁检查）| Open door (with interlock check)
|
||||
/// 联锁信号有效时拒绝开门 | Rejects when interlock is active
|
||||
/// </summary>
|
||||
/// <returns>操作结果 | Operation result</returns>
|
||||
MotionResult OpenDoor();
|
||||
|
||||
/// <summary>
|
||||
/// 关门 | Close door
|
||||
/// </summary>
|
||||
/// <returns>操作结果 | Operation result</returns>
|
||||
MotionResult CloseDoor();
|
||||
|
||||
/// <summary>
|
||||
/// 停止门运动 | Stop door movement
|
||||
/// </summary>
|
||||
/// <returns>操作结果 | Operation result</returns>
|
||||
MotionResult StopDoor();
|
||||
|
||||
#endregion
|
||||
|
||||
#region 几何计算 | Geometry Calculation
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前几何参数（正算）| Get current geometry parameters (forward calculation)
|
||||
/// 根据当前轴位置计算 FOD、FDD 和放大倍率 | Calculates FOD, FDD and magnification from current axis positions
|
||||
/// </summary>
|
||||
/// <returns>FOD（mm）、FDD（mm）、放大倍率 | FOD (mm), FDD (mm), Magnification</returns>
|
||||
(double FOD, double FDD, double Magnification) GetCurrentGeometry();
|
||||
|
||||
/// <summary>
|
||||
/// 应用几何参数（反算，由 FOD 和 FDD）| Apply geometry (inverse, from FOD and FDD)
|
||||
/// 计算并移动 SourceZ 和 DetectorZ 到目标位置 | Calculates and moves SourceZ and DetectorZ to target positions
|
||||
/// </summary>
|
||||
/// <param name="targetFOD">目标 FOD（mm）| Target FOD (mm)</param>
|
||||
/// <param name="targetFDD">目标 FDD（mm）| Target FDD (mm)</param>
|
||||
/// <returns>操作结果 | Operation result</returns>
|
||||
MotionResult ApplyGeometry(double targetFOD, double targetFDD);
|
||||
|
||||
/// <summary>
|
||||
/// 几何反算（仅计算，不移动）| Geometry inverse calculation (calculate only, no move)
|
||||
/// 返回 SourceZ 和 DetectorZ 的目标位置 | Returns target positions for SourceZ and DetectorZ
|
||||
/// </summary>
|
||||
/// <param name="targetFOD">目标 FOD（mm）| Target FOD (mm)</param>
|
||||
/// <param name="targetFDD">目标 FDD（mm）| Target FDD (mm)</param>
|
||||
/// <returns>成功时返回 (sourceZTarget, detectorZTarget, null)，失败时返回 (0, 0, errorMessage) | On success returns targets, on failure returns error</returns>
|
||||
(double SourceZTarget, double DetectorZTarget, string ErrorMessage) CalculateGeometryTargets(double targetFOD, double targetFDD);
|
||||
|
||||
/// <summary>
|
||||
/// 应用几何参数（反算，由 FOD 和放大倍率）| Apply geometry (inverse, from FOD and magnification)
|
||||
/// 先计算 FDD = M × FOD，再移动轴 | Calculates FDD = M × FOD, then moves axes
|
||||
/// </summary>
|
||||
/// <param name="targetFOD">目标 FOD（mm）| Target FOD (mm)</param>
|
||||
/// <param name="magnification">目标放大倍率 | Target magnification</param>
|
||||
/// <returns>操作结果 | Operation result</returns>
|
||||
MotionResult ApplyGeometryByMagnification(double targetFOD, double magnification);
|
||||
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
@@ -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