|
|
@@ -1,8 +1,8 @@
|
|
|
using System;
|
|
|
using System.Collections.Generic;
|
|
|
+using System.IO;
|
|
|
using System.Linq;
|
|
|
-using System.Text;
|
|
|
-using System.Threading.Tasks;
|
|
|
+using TeamAAS;
|
|
|
using TeamAAS.Global.Devices;
|
|
|
using TeamAAS.Motion.Models;
|
|
|
using System.Runtime.InteropServices;
|
|
|
@@ -10,58 +10,1205 @@ using System.Runtime.InteropServices;
|
|
|
namespace TeamAAS.Motion.Motions
|
|
|
{
|
|
|
/// <summary>
|
|
|
- /// 汇川IMC60运动控制卡
|
|
|
+ /// 汇川 IMC60G 运动控制卡(对接 IMC_Library_x64.dll)。
|
|
|
+ /// 流程层用索引 API;调试层用 <see cref="InovanceAxis"/>。
|
|
|
/// </summary>
|
|
|
- public class InovanceMotionCard : IMotionCard
|
|
|
+ public class InovanceMotionCard : IMotionCard, IMasterFollowHost, IMotionSafetyHost, IMotionIoHost
|
|
|
{
|
|
|
+ private const short StopSmooth = 0;
|
|
|
+ private const short StopAbrupt = 1;
|
|
|
+ private const uint EcatMasterOp = 6;
|
|
|
private readonly MotionDeviceConfig _cfg;
|
|
|
+ private readonly object _sync = new object();
|
|
|
+ private readonly MasterFollowSession _follow = new MasterFollowSession();
|
|
|
+ private readonly MotionSafetyGuard _guard;
|
|
|
+ private short _card;
|
|
|
+ private int _axisCount;
|
|
|
+ private IAxis[] _axes = Array.Empty<IAxis>();
|
|
|
+ private Dictionary<int, IAxis> _axisMap = new Dictionary<int, IAxis>();
|
|
|
+ private Dictionary<int, InovanceAxis> _hwAxes = new Dictionary<int, InovanceAxis>();
|
|
|
+ private Dictionary<int, MotionAxisConfig> _axisCfgs = new Dictionary<int, MotionAxisConfig>();
|
|
|
+ private bool _crdReady;
|
|
|
+ private int _crdDim;
|
|
|
+ private int _ecatDiCnt;
|
|
|
+ private int _ecatDoCnt;
|
|
|
|
|
|
public string Name => _cfg.Name;
|
|
|
public bool IsConnected { get; private set; }
|
|
|
- public int AxisCount => _cfg.AxisCount;
|
|
|
+ public int AxisCount => _axisCount;
|
|
|
+ public bool IsEstopActive => _guard.EstopActive;
|
|
|
+ public bool IsStopActive => _guard.StopActive;
|
|
|
+ internal MotionDeviceConfig DeviceConfig => _cfg;
|
|
|
|
|
|
public event EventHandler<MotionConnectionStateChangedEventArgs> ConnectionStateChanged;
|
|
|
public event EventHandler<MotionErrorEventArgs> ErrorOccurred;
|
|
|
+ public event EventHandler SafetyChanged;
|
|
|
|
|
|
public InovanceMotionCard(MotionDeviceConfig cfg)
|
|
|
{
|
|
|
_cfg = cfg ?? new MotionDeviceConfig();
|
|
|
+ _card = (short)_cfg.BoardIndex;
|
|
|
+ _axisCount = Math.Max(1, _cfg.AxisCount);
|
|
|
+ RebuildAxes();
|
|
|
+ _guard = new MotionSafetyGuard(
|
|
|
+ () => _cfg,
|
|
|
+ ReadDI,
|
|
|
+ OnSafetyEstopRise,
|
|
|
+ OnSafetyEstopHold,
|
|
|
+ OnSafetyStopRise);
|
|
|
+ _guard.Changed += (_, __) =>
|
|
|
+ {
|
|
|
+ try { SafetyChanged?.Invoke(this, EventArgs.Empty); }
|
|
|
+ catch { }
|
|
|
+ };
|
|
|
+ }
|
|
|
+
|
|
|
+ public bool AllowEnable(out string reason) => _guard.AllowEnable(out reason);
|
|
|
+ public bool AllowMotion(out string reason) => _guard.AllowMotion(out reason);
|
|
|
+
|
|
|
+ private bool RejectEnable()
|
|
|
+ {
|
|
|
+ if (_guard.AllowEnable(out var reason)) return false;
|
|
|
+ RaiseError(reason);
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+
|
|
|
+ private bool RejectMotion()
|
|
|
+ {
|
|
|
+ if (_guard.AllowMotion(out var reason)) return false;
|
|
|
+ RaiseError(reason);
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+
|
|
|
+ private void OnSafetyEstopRise()
|
|
|
+ {
|
|
|
+ if (!IsConnected) return;
|
|
|
+ EmergencyStop();
|
|
|
+ }
|
|
|
+
|
|
|
+ private void OnSafetyEstopHold()
|
|
|
+ {
|
|
|
+ if (!IsConnected) return;
|
|
|
+ lock (_sync)
|
|
|
+ {
|
|
|
+ if (!IsConnected) return;
|
|
|
+ foreach (var hw in _hwAxes.Values)
|
|
|
+ {
|
|
|
+ try { Imc60.IMC_ServoOff(_card, (short)hw.AxisNo, 1); }
|
|
|
+ catch { }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ private void OnSafetyStopRise()
|
|
|
+ {
|
|
|
+ if (!IsConnected) return;
|
|
|
+ Stop(-1);
|
|
|
}
|
|
|
|
|
|
public bool Open()
|
|
|
{
|
|
|
- uint ret = Imc60.IMC_OpenCard((short)_cfg.BoardIndex);
|
|
|
- IsConnected = ret == Imc60.EXE_SUCCESS;
|
|
|
- if (IsConnected)
|
|
|
- ConnectionStateChanged?.Invoke(this, new MotionConnectionStateChangedEventArgs(true));
|
|
|
- return IsConnected;
|
|
|
+ lock (_sync)
|
|
|
+ {
|
|
|
+ if (IsConnected) return true;
|
|
|
+ try
|
|
|
+ {
|
|
|
+ uint ret = Imc60.IMC_OpenCard(_card);
|
|
|
+ if (!Ok(ret, "开卡")) return false;
|
|
|
+
|
|
|
+ ParseConfigPaths(out var deviceCfg, out var systemCfg);
|
|
|
+ if (!string.IsNullOrEmpty(deviceCfg))
|
|
|
+ {
|
|
|
+ if (!File.Exists(deviceCfg))
|
|
|
+ {
|
|
|
+ RaiseError("设备配置不存在: " + deviceCfg);
|
|
|
+ return FailOpen();
|
|
|
+ }
|
|
|
+ if (!Ok(Imc60.IMC_DownLoadDeviceConfig(_card, deviceCfg), "下载设备配置"))
|
|
|
+ return FailOpen();
|
|
|
+ }
|
|
|
+
|
|
|
+ short masterCfg = 0;
|
|
|
+ Imc60.IMC_GetMasterCfgXml(_card, ref masterCfg);
|
|
|
+ if ((masterCfg & 0x1) != 0)
|
|
|
+ {
|
|
|
+ uint sts = 0;
|
|
|
+ Imc60.IMC_GetEcatMasterSts(_card, ref sts);
|
|
|
+ if (sts != EcatMasterOp)
|
|
|
+ {
|
|
|
+ if (!Ok(Imc60.IMC_ScanCardEcat(_card), "EtherCAT0 扫描"))
|
|
|
+ return FailOpen();
|
|
|
+ }
|
|
|
+ if (!string.IsNullOrEmpty(systemCfg))
|
|
|
+ {
|
|
|
+ if (!File.Exists(systemCfg))
|
|
|
+ {
|
|
|
+ RaiseError("系统配置不存在: " + systemCfg);
|
|
|
+ return FailOpen();
|
|
|
+ }
|
|
|
+ if (!Ok(Imc60.IMC_DownLoadSystemConfig(_card, systemCfg), "下载系统配置"))
|
|
|
+ return FailOpen();
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if ((masterCfg & 0x2) != 0)
|
|
|
+ {
|
|
|
+ uint sts = 0;
|
|
|
+ Imc60.IMC_H_GetEcatMasterSts(_card, ref sts);
|
|
|
+ if (sts != EcatMasterOp)
|
|
|
+ {
|
|
|
+ if (!Ok(Imc60.IMC_H_ScanCardEcat(_card), "EtherCAT1 扫描"))
|
|
|
+ return FailOpen();
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ bool userAxes = _cfg.Axes != null && _cfg.Axes.Count > 0;
|
|
|
+ var info = new Imc60.TMasterInfo();
|
|
|
+ bool haveInfo = Imc60.IMC_GetEcatMasterInfo(_card, ref info) == Imc60.EXE_SUCCESS;
|
|
|
+ if (haveInfo)
|
|
|
+ {
|
|
|
+ _ecatDiCnt = Math.Max(0, (int)info.diCnt);
|
|
|
+ _ecatDoCnt = Math.Max(0, (int)info.doCnt);
|
|
|
+ }
|
|
|
+ if (!userAxes)
|
|
|
+ {
|
|
|
+ if (haveInfo && info.axisCnt > 0)
|
|
|
+ _axisCount = info.axisCnt;
|
|
|
+ else
|
|
|
+ _axisCount = Math.Max(1, _cfg.AxisCount);
|
|
|
+ }
|
|
|
+
|
|
|
+ RebuildAxes();
|
|
|
+ ApplyAxisBonds();
|
|
|
+ ApplySafetyLimits();
|
|
|
+ foreach (var hw in _hwAxes.Values)
|
|
|
+ {
|
|
|
+ Imc60.IMC_SetAxMvPara(_card, (short)hw.AxisNo, hw.Velocity, hw.Acceleration, hw.Deceleration);
|
|
|
+ Imc60.IMC_SetAxStopDec(_card, (short)hw.AxisNo, hw.Deceleration, hw.Deceleration * 4);
|
|
|
+ }
|
|
|
+
|
|
|
+ IsConnected = true;
|
|
|
+ }
|
|
|
+ catch (DllNotFoundException)
|
|
|
+ {
|
|
|
+ RaiseError("未找到 IMC_Library_x64.dll,请放到程序运行目录");
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ catch (Exception ex)
|
|
|
+ {
|
|
|
+ RaiseError("开卡异常: " + ex.Message);
|
|
|
+ return FailOpen();
|
|
|
+ }
|
|
|
+ }
|
|
|
+ _guard.Start();
|
|
|
+ ConnectionStateChanged?.Invoke(this, new MotionConnectionStateChangedEventArgs(true));
|
|
|
+ return true;
|
|
|
}
|
|
|
|
|
|
public void Close()
|
|
|
+ {
|
|
|
+ _guard.Stop();
|
|
|
+ lock (_sync)
|
|
|
+ {
|
|
|
+ if (!IsConnected) return;
|
|
|
+ try
|
|
|
+ {
|
|
|
+ foreach (var hw in _hwAxes.Values)
|
|
|
+ Imc60.IMC_ServoOff(_card, (short)hw.AxisNo, 1);
|
|
|
+ if (_crdReady)
|
|
|
+ {
|
|
|
+ Imc60.IMC_CrdStop(_card, 0, StopAbrupt);
|
|
|
+ Imc60.IMC_CrdDeleteMtSys(_card, 0);
|
|
|
+ _crdReady = false;
|
|
|
+ }
|
|
|
+ Imc60.IMC_DelEcatComm(_card);
|
|
|
+ Imc60.IMC_CloseCard(_card);
|
|
|
+ }
|
|
|
+ catch { }
|
|
|
+ _follow.Clear();
|
|
|
+ IsConnected = false;
|
|
|
+ }
|
|
|
+ ConnectionStateChanged?.Invoke(this, new MotionConnectionStateChangedEventArgs(false));
|
|
|
+ }
|
|
|
+
|
|
|
+ void IMasterFollowHost.BeginSoloMasterFollow(int masterAxisNo, IEnumerable<IAxis> slaves)
|
|
|
+ => _follow.Begin(masterAxisNo, slaves);
|
|
|
+
|
|
|
+ void IMasterFollowHost.ReleaseMasterFollow() => _follow.Release();
|
|
|
+
|
|
|
+ IDisposable IMasterFollowHost.SuspendFollowRelease() => _follow.Suspend();
|
|
|
+
|
|
|
+ private void ReleaseFollowIfIndependent(int axis)
|
|
|
+ {
|
|
|
+ if (_follow.Suspended || _follow.IsReleasing) return;
|
|
|
+ if (axis >= 0 && TryCombined(axis, out _)) return;
|
|
|
+ _follow.Release();
|
|
|
+ }
|
|
|
+
|
|
|
+ public bool SetEnable(int axis, bool enable)
|
|
|
+ {
|
|
|
+ if (enable && RejectEnable()) return false;
|
|
|
+ if (!IsConnected) return false;
|
|
|
+ lock (_sync)
|
|
|
+ {
|
|
|
+ if (axis < 0)
|
|
|
+ {
|
|
|
+ bool ok = true;
|
|
|
+ foreach (var hw in _hwAxes.Values)
|
|
|
+ {
|
|
|
+ if (enable)
|
|
|
+ Imc60.IMC_ClrAxSts(_card, (short)hw.AxisNo, 1);
|
|
|
+ uint r = enable
|
|
|
+ ? Imc60.IMC_ServoOn(_card, (short)hw.AxisNo, 1)
|
|
|
+ : Imc60.IMC_ServoOff(_card, (short)hw.AxisNo, 1);
|
|
|
+ if (!Ok(r, enable ? $"轴{hw.AxisNo}使能" : $"轴{hw.AxisNo}下使能"))
|
|
|
+ ok = false;
|
|
|
+ }
|
|
|
+ return ok;
|
|
|
+ }
|
|
|
+ if (TryCombined(axis, out var combined))
|
|
|
+ {
|
|
|
+ if (enable) combined.Enable(); else combined.Disable();
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ if (!IsHardware(axis)) return false;
|
|
|
+ if (enable)
|
|
|
+ Imc60.IMC_ClrAxSts(_card, (short)axis, 1);
|
|
|
+ uint ret = enable
|
|
|
+ ? Imc60.IMC_ServoOn(_card, (short)axis, 1)
|
|
|
+ : Imc60.IMC_ServoOff(_card, (short)axis, 1);
|
|
|
+ return Ok(ret, enable ? $"轴{axis}使能" : $"轴{axis}下使能");
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ public bool Home(int axis, int mode = 0)
|
|
|
+ {
|
|
|
+ if (RejectMotion()) return false;
|
|
|
+ if (!IsConnected || !IsValidAxis(axis)) return false;
|
|
|
+ lock (_sync)
|
|
|
+ {
|
|
|
+ if (TryCombined(axis, out var combined))
|
|
|
+ {
|
|
|
+ combined.Home((HomeMode)mode);
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ ReleaseFollowIfIndependent(axis);
|
|
|
+ if (!_hwAxes.TryGetValue(axis, out var a)) return false;
|
|
|
+ var para = new Imc60.THomingPara
|
|
|
+ {
|
|
|
+ homeMethod = MapHomeMethod(mode),
|
|
|
+ highVel = ToUInt(Math.Abs(a.Velocity)),
|
|
|
+ lowVel = ToUInt(Math.Abs(a.Velocity) * 0.2),
|
|
|
+ acc = ToUInt(Math.Max(1, a.Acceleration)),
|
|
|
+ offset = 0
|
|
|
+ };
|
|
|
+ if (!Ok(Imc60.IMC_StartHoming(_card, (short)axis, ref para), $"轴{axis}回零"))
|
|
|
+ return false;
|
|
|
+ a.MarkHoming();
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ public bool IsHomed(int axis)
|
|
|
+ {
|
|
|
+ if (!IsValidAxis(axis)) return false;
|
|
|
+ RefreshHomeFlag(axis);
|
|
|
+ return _axisMap[axis].IsHomed;
|
|
|
+ }
|
|
|
+
|
|
|
+ public bool MoveAbsolute(int axis, double position, double velocity)
|
|
|
+ {
|
|
|
+ if (RejectMotion()) return false;
|
|
|
+ if (!IsConnected || !IsValidAxis(axis)) return false;
|
|
|
+ lock (_sync)
|
|
|
+ {
|
|
|
+ if (TryCombined(axis, out var combined))
|
|
|
+ {
|
|
|
+ combined.Velocity = velocity;
|
|
|
+ combined.MoveTo(position);
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ ReleaseFollowIfIndependent(axis);
|
|
|
+ position = ClampAxisPosition(axis, position);
|
|
|
+ velocity = Math.Abs(ClampAxisVelocity(axis, velocity));
|
|
|
+ if (!ApplyMovePara(axis, velocity)) return false;
|
|
|
+ return Ok(Imc60.IMC_StartPtpMove(_card, (short)axis, position, 0), $"轴{axis}绝对定位");
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ public bool MoveRelative(int axis, double distance, double velocity)
|
|
|
+ {
|
|
|
+ if (RejectMotion()) return false;
|
|
|
+ if (!IsConnected || !IsValidAxis(axis)) return false;
|
|
|
+ lock (_sync)
|
|
|
+ {
|
|
|
+ if (TryCombined(axis, out var combined))
|
|
|
+ {
|
|
|
+ combined.Velocity = velocity;
|
|
|
+ combined.MoveBy(distance);
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ ReleaseFollowIfIndependent(axis);
|
|
|
+ double current = ReadPrfPos(axis);
|
|
|
+ double target = ClampAxisPosition(axis, current + distance);
|
|
|
+ velocity = Math.Abs(ClampAxisVelocity(axis, velocity));
|
|
|
+ if (!ApplyMovePara(axis, velocity)) return false;
|
|
|
+ return Ok(Imc60.IMC_StartPtpMove(_card, (short)axis, target, 0), $"轴{axis}相对定位");
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ public bool JogStart(int axis, int direction, double velocity)
|
|
|
+ {
|
|
|
+ if (RejectMotion()) return false;
|
|
|
+ if (!IsConnected || !IsValidAxis(axis)) return false;
|
|
|
+ lock (_sync)
|
|
|
+ {
|
|
|
+ double vel = (direction >= 0 ? 1 : -1) * Math.Abs(velocity);
|
|
|
+ if (TryCombined(axis, out var combined))
|
|
|
+ {
|
|
|
+ combined.Jog(vel);
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ ReleaseFollowIfIndependent(axis);
|
|
|
+ vel = ClampAxisJog(axis, vel, ReadPrfPos(axis));
|
|
|
+ if (Math.Abs(vel) < 1e-9)
|
|
|
+ return Stop(axis);
|
|
|
+ if (!ApplyMovePara(axis, Math.Abs(vel))) return false;
|
|
|
+ if (_hwAxes.TryGetValue(axis, out var hw))
|
|
|
+ hw.MarkJogging(true);
|
|
|
+ return Ok(Imc60.IMC_StartJogMove(_card, (short)axis, vel), $"轴{axis}点动");
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ public bool JogStop(int axis) => Stop(axis);
|
|
|
+
|
|
|
+ public bool Stop(int axis = -1)
|
|
|
+ {
|
|
|
+ if (!IsConnected) return false;
|
|
|
+ lock (_sync)
|
|
|
+ {
|
|
|
+ if (axis < 0)
|
|
|
+ {
|
|
|
+ _follow.Release();
|
|
|
+ bool ok = true;
|
|
|
+ foreach (var hw in _hwAxes.Values)
|
|
|
+ {
|
|
|
+ hw.AbortHome();
|
|
|
+ if (!Ok(Imc60.IMC_StopMove(_card, (short)hw.AxisNo, StopSmooth), $"轴{hw.AxisNo}停止"))
|
|
|
+ ok = false;
|
|
|
+ hw.MarkJogging(false);
|
|
|
+ }
|
|
|
+ return ok;
|
|
|
+ }
|
|
|
+ if (TryCombined(axis, out var combined))
|
|
|
+ {
|
|
|
+ combined.Stop();
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ ReleaseFollowIfIndependent(axis);
|
|
|
+ if (!IsHardware(axis)) return false;
|
|
|
+ if (_hwAxes.TryGetValue(axis, out var a))
|
|
|
+ {
|
|
|
+ a.AbortHome();
|
|
|
+ a.MarkJogging(false);
|
|
|
+ }
|
|
|
+ return Ok(Imc60.IMC_StopMove(_card, (short)axis, StopSmooth), $"轴{axis}停止");
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ public AxisStatus GetAxisStatus(int axis)
|
|
|
+ {
|
|
|
+ if (!IsValidAxis(axis)) return null;
|
|
|
+ var logical = _axisMap[axis];
|
|
|
+ if (logical is CombinedMotionAxis)
|
|
|
+ {
|
|
|
+ return new AxisStatus
|
|
|
+ {
|
|
|
+ Index = axis,
|
|
|
+ Name = logical.Name,
|
|
|
+ Position = logical.ActualPosition,
|
|
|
+ Velocity = logical.ActualVelocity,
|
|
|
+ IsMoving = logical.IsMoving,
|
|
|
+ IsHomed = logical.IsHomed,
|
|
|
+ IsEnabled = logical.IsEnabled
|
|
|
+ };
|
|
|
+ }
|
|
|
+
|
|
|
+ RefreshHomeFlag(axis);
|
|
|
+ if (logical is InovanceAxis inv)
|
|
|
+ inv.SyncState();
|
|
|
+ int sts = ReadSts(axis);
|
|
|
+ return new AxisStatus
|
|
|
+ {
|
|
|
+ Index = axis,
|
|
|
+ Name = logical.Name,
|
|
|
+ Position = ReadEncPos(axis),
|
|
|
+ Velocity = ReadEncVel(axis),
|
|
|
+ IsMoving = logical.IsMoving,
|
|
|
+ IsHomed = logical.IsHomed,
|
|
|
+ IsEnabled = logical.IsEnabled,
|
|
|
+ PositiveLimit = Has(sts, Imc60.AX_POSLMT_BIT) || Has(sts, Imc60.AX_SOFT_POSLMT_BIT),
|
|
|
+ NegativeLimit = Has(sts, Imc60.AX_NEGLMT_BIT) || Has(sts, Imc60.AX_SOFT_NEGLMT_BIT),
|
|
|
+ Alarm = Has(sts, Imc60.AX_ALARM_BIT) || Has(sts, Imc60.AX_UNLINK_BIT) || Has(sts, Imc60.AX_ERRPOS_BIT)
|
|
|
+ };
|
|
|
+ }
|
|
|
+
|
|
|
+ public List<AxisStatus> GetAllAxisStatus()
|
|
|
+ {
|
|
|
+ var list = new List<AxisStatus>(_axes.Length);
|
|
|
+ foreach (var a in _axes)
|
|
|
+ list.Add(GetAxisStatus(a.AxisNo));
|
|
|
+ return list;
|
|
|
+ }
|
|
|
+
|
|
|
+ public IAxis GetAxis(int axisNo)
|
|
|
+ {
|
|
|
+ if (!IsValidAxis(axisNo))
|
|
|
+ throw new ArgumentOutOfRangeException(nameof(axisNo), "轴号超出范围");
|
|
|
+ return _axisMap[axisNo];
|
|
|
+ }
|
|
|
+
|
|
|
+ public IList<IAxis> GetAxes() => _axes.ToList();
|
|
|
+
|
|
|
+ public bool MoveLinear(double[] targetPositions, double velocity)
|
|
|
+ {
|
|
|
+ if (RejectMotion()) return false;
|
|
|
+ if (!IsConnected || targetPositions == null || targetPositions.Length < 2) return false;
|
|
|
+ lock (_sync)
|
|
|
+ {
|
|
|
+ _follow.Release();
|
|
|
+ int dim = targetPositions.Length >= 3 ? 3 : 2;
|
|
|
+ velocity = ClampInterpVelocity(velocity, dim);
|
|
|
+ if (!EnsureCrd(dim)) return false;
|
|
|
+ if (!PrepareCrd(velocity)) return false;
|
|
|
+
|
|
|
+ if (dim >= 3)
|
|
|
+ {
|
|
|
+ var ends = new[]
|
|
|
+ {
|
|
|
+ ClampAxisPosition(0, targetPositions[0]),
|
|
|
+ ClampAxisPosition(1, targetPositions[1]),
|
|
|
+ ClampAxisPosition(2, targetPositions[2])
|
|
|
+ };
|
|
|
+ if (!Ok(Imc60.IMC_CrdLineXYZ(_card, 0, ends), "插补直线XYZ")) return false;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ var ends = new[]
|
|
|
+ {
|
|
|
+ ClampAxisPosition(0, targetPositions[0]),
|
|
|
+ ClampAxisPosition(1, targetPositions[1])
|
|
|
+ };
|
|
|
+ if (!Ok(Imc60.IMC_CrdLineXY(_card, 0, ends), "插补直线XY")) return false;
|
|
|
+ }
|
|
|
+ return Ok(Imc60.IMC_CrdStart(_card, 0), "启动插补");
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ public bool MoveArc(double[] centerPoint, double[] targetPositions, double velocity)
|
|
|
+ {
|
|
|
+ if (RejectMotion()) return false;
|
|
|
+ if (!IsConnected || centerPoint == null || targetPositions == null) return false;
|
|
|
+ if (centerPoint.Length < 2 || targetPositions.Length < 2) return false;
|
|
|
+ lock (_sync)
|
|
|
+ {
|
|
|
+ _follow.Release();
|
|
|
+ velocity = ClampInterpVelocity(velocity, 2);
|
|
|
+ if (!EnsureCrd(2)) return false;
|
|
|
+ if (!PrepareCrd(velocity)) return false;
|
|
|
+
|
|
|
+ var center = new[] { centerPoint[0], centerPoint[1] };
|
|
|
+ var ends = new[]
|
|
|
+ {
|
|
|
+ ClampAxisPosition(0, targetPositions[0]),
|
|
|
+ ClampAxisPosition(1, targetPositions[1])
|
|
|
+ };
|
|
|
+ if (!Ok(Imc60.IMC_CrdArcCenterXYPlane(_card, 0, center, ends, 0), "插补圆弧")) return false;
|
|
|
+ return Ok(Imc60.IMC_CrdStart(_card, 0), "启动插补");
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ public bool EmergencyStop()
|
|
|
+ {
|
|
|
+ if (!IsConnected) return false;
|
|
|
+ lock (_sync)
|
|
|
+ {
|
|
|
+ _follow.Clear();
|
|
|
+ Imc60.IMC_CrdStop(_card, 0, StopAbrupt);
|
|
|
+ bool ok = true;
|
|
|
+ foreach (var hw in _hwAxes.Values)
|
|
|
+ {
|
|
|
+ hw.AbortHome();
|
|
|
+ if (!Ok(Imc60.IMC_StopMove(_card, (short)hw.AxisNo, StopAbrupt), $"轴{hw.AxisNo}急停"))
|
|
|
+ ok = false;
|
|
|
+ hw.MarkJogging(false);
|
|
|
+ }
|
|
|
+ return ok;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ public bool CanSimulateDi => false;
|
|
|
+
|
|
|
+ public int GetDiCount(MotionIoBank bank)
|
|
|
+ {
|
|
|
+ if (bank == MotionIoBank.EtherCat)
|
|
|
+ return _ecatDiCnt > 0 ? _ecatDiCnt : Math.Max(0, _cfg.InputCount);
|
|
|
+ int n = _cfg.LocalInputCount > 0 ? _cfg.LocalInputCount : 16;
|
|
|
+ return n;
|
|
|
+ }
|
|
|
+
|
|
|
+ public int GetDoCount(MotionIoBank bank)
|
|
|
+ {
|
|
|
+ if (bank == MotionIoBank.EtherCat)
|
|
|
+ return _ecatDoCnt > 0 ? _ecatDoCnt : Math.Max(0, _cfg.OutputCount);
|
|
|
+ int n = _cfg.LocalOutputCount > 0 ? _cfg.LocalOutputCount : 16;
|
|
|
+ return n;
|
|
|
+ }
|
|
|
+
|
|
|
+ public bool ReadDI(int portIndex) => ReadDI(MotionIoBank.Local, portIndex);
|
|
|
+
|
|
|
+ public bool ReadDI(MotionIoBank bank, int index)
|
|
|
+ {
|
|
|
+ if (!IsConnected || index < 0) return false;
|
|
|
+ short v = 0;
|
|
|
+ uint ret = bank == MotionIoBank.EtherCat
|
|
|
+ ? Imc60.IMC_GetEcatDiBit(_card, (short)index, ref v)
|
|
|
+ : Imc60.IMC_GetLocalDiBit(_card, (short)index, ref v);
|
|
|
+ return ret == Imc60.EXE_SUCCESS && v != 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ public bool ReadDO(MotionIoBank bank, int index)
|
|
|
+ {
|
|
|
+ if (!IsConnected || index < 0) return false;
|
|
|
+ short v = 0;
|
|
|
+ uint ret = bank == MotionIoBank.EtherCat
|
|
|
+ ? Imc60.IMC_GetEcatDoBit(_card, (short)index, ref v)
|
|
|
+ : Imc60.IMC_GetLocalDoBit(_card, (short)index, ref v);
|
|
|
+ return ret == Imc60.EXE_SUCCESS && v != 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ public bool WriteDO(int portIndex, bool value) => WriteDO(MotionIoBank.Local, portIndex, value);
|
|
|
+
|
|
|
+ public bool WriteDO(MotionIoBank bank, int index, bool value)
|
|
|
+ {
|
|
|
+ if (!IsConnected || index < 0) return false;
|
|
|
+ short bit = (short)(value ? 1 : 0);
|
|
|
+ uint ret = bank == MotionIoBank.EtherCat
|
|
|
+ ? Imc60.IMC_SetEcatDoBit(_card, (short)index, bit)
|
|
|
+ : Imc60.IMC_SetLocalDoBit(_card, (short)index, bit);
|
|
|
+ return Ok(ret, bank == MotionIoBank.EtherCat ? $"写 EtherCAT DO{index}" : $"写本地 DO{index}");
|
|
|
+ }
|
|
|
+
|
|
|
+ public bool WriteDI(MotionIoBank bank, int index, bool value) => false;
|
|
|
+
|
|
|
+ internal bool ApplyMovePara(int axis, double velocity)
|
|
|
+ {
|
|
|
+ if (!_hwAxes.TryGetValue(axis, out var a)) return false;
|
|
|
+ if (velocity > 0) a.Velocity = velocity;
|
|
|
+ return Ok(Imc60.IMC_SetAxMvPara(_card, (short)axis, a.Velocity, a.Acceleration, a.Deceleration), $"轴{axis}运动参数");
|
|
|
+ }
|
|
|
+
|
|
|
+ internal bool HardwareSetEnable(int axis, bool enable)
|
|
|
+ {
|
|
|
+ if (enable && RejectEnable()) return false;
|
|
|
+ if (!IsConnected || !_hwAxes.ContainsKey(axis)) return false;
|
|
|
+ lock (_sync)
|
|
|
+ {
|
|
|
+ if (enable)
|
|
|
+ Imc60.IMC_ClrAxSts(_card, (short)axis, 1);
|
|
|
+ uint ret = enable
|
|
|
+ ? Imc60.IMC_ServoOn(_card, (short)axis, 1)
|
|
|
+ : Imc60.IMC_ServoOff(_card, (short)axis, 1);
|
|
|
+ return Ok(ret, enable ? $"轴{axis}使能" : $"轴{axis}下使能");
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ internal bool HardwareMoveAbsolute(int axis, double position, double velocity)
|
|
|
+ {
|
|
|
+ if (RejectMotion()) return false;
|
|
|
+ if (!IsConnected || !_hwAxes.ContainsKey(axis)) return false;
|
|
|
+ lock (_sync)
|
|
|
+ {
|
|
|
+ position = ClampAxisPosition(axis, position);
|
|
|
+ velocity = Math.Abs(ClampAxisVelocity(axis, velocity));
|
|
|
+ if (!ApplyMovePara(axis, velocity)) return false;
|
|
|
+ return Ok(Imc60.IMC_StartPtpMove(_card, (short)axis, position, 0), $"轴{axis}绝对定位");
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ internal bool HardwareMoveRelative(int axis, double distance, double velocity)
|
|
|
+ {
|
|
|
+ if (RejectMotion()) return false;
|
|
|
+ if (!IsConnected || !_hwAxes.ContainsKey(axis)) return false;
|
|
|
+ lock (_sync)
|
|
|
+ {
|
|
|
+ double target = ClampAxisPosition(axis, ReadPrfPos(axis) + distance);
|
|
|
+ velocity = Math.Abs(ClampAxisVelocity(axis, velocity));
|
|
|
+ if (!ApplyMovePara(axis, velocity)) return false;
|
|
|
+ return Ok(Imc60.IMC_StartPtpMove(_card, (short)axis, target, 0), $"轴{axis}相对定位");
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ internal bool HardwareJog(int axis, double velocity)
|
|
|
+ {
|
|
|
+ if (RejectMotion()) return false;
|
|
|
+ if (!IsConnected || !_hwAxes.TryGetValue(axis, out var hw)) return false;
|
|
|
+ lock (_sync)
|
|
|
+ {
|
|
|
+ velocity = ClampAxisJog(axis, velocity, ReadPrfPos(axis));
|
|
|
+ if (Math.Abs(velocity) < 1e-9)
|
|
|
+ {
|
|
|
+ hw.MarkJogging(false);
|
|
|
+ return Ok(Imc60.IMC_StopMove(_card, (short)axis, StopSmooth), $"轴{axis}停止");
|
|
|
+ }
|
|
|
+ if (!ApplyMovePara(axis, Math.Abs(velocity))) return false;
|
|
|
+ hw.MarkJogging(true);
|
|
|
+ return Ok(Imc60.IMC_StartJogMove(_card, (short)axis, velocity), $"轴{axis}点动");
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ internal bool HardwareStop(int axis)
|
|
|
+ {
|
|
|
+ if (!IsConnected || !_hwAxes.TryGetValue(axis, out var hw)) return false;
|
|
|
+ lock (_sync)
|
|
|
+ {
|
|
|
+ hw.AbortHome();
|
|
|
+ hw.MarkJogging(false);
|
|
|
+ return Ok(Imc60.IMC_StopMove(_card, (short)axis, StopSmooth), $"轴{axis}停止");
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ internal bool HardwareHome(int axis, int mode)
|
|
|
+ {
|
|
|
+ if (RejectMotion()) return false;
|
|
|
+ if (!IsConnected || !_hwAxes.TryGetValue(axis, out var a)) return false;
|
|
|
+ lock (_sync)
|
|
|
+ {
|
|
|
+ var para = new Imc60.THomingPara
|
|
|
+ {
|
|
|
+ homeMethod = MapHomeMethod(mode),
|
|
|
+ highVel = ToUInt(Math.Abs(a.Velocity)),
|
|
|
+ lowVel = ToUInt(Math.Abs(a.Velocity) * 0.2),
|
|
|
+ acc = ToUInt(Math.Max(1, a.Acceleration)),
|
|
|
+ offset = 0
|
|
|
+ };
|
|
|
+ if (!Ok(Imc60.IMC_StartHoming(_card, (short)axis, ref para), $"轴{axis}回零"))
|
|
|
+ return false;
|
|
|
+ a.MarkHoming();
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ internal bool SetCurrentPosition(int axis, double position)
|
|
|
+ => Ok(Imc60.IMC_SetAxCurPos(_card, (short)axis, position), $"轴{axis}位置清零");
|
|
|
+
|
|
|
+ internal int ReadSts(int axis)
|
|
|
+ {
|
|
|
+ var arr = new int[1];
|
|
|
+ return Imc60.IMC_GetAxSts(_card, (short)axis, arr, 1) == Imc60.EXE_SUCCESS ? arr[0] : 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ internal double ReadEncPos(int axis)
|
|
|
+ {
|
|
|
+ var arr = new double[1];
|
|
|
+ return Imc60.IMC_GetAxEncPos(_card, (short)axis, arr, 1) == Imc60.EXE_SUCCESS ? arr[0] : 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ internal double ReadPrfPos(int axis)
|
|
|
+ {
|
|
|
+ var arr = new double[1];
|
|
|
+ return Imc60.IMC_GetAxPrfPos(_card, (short)axis, arr, 1) == Imc60.EXE_SUCCESS ? arr[0] : 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ internal double ReadEncVel(int axis)
|
|
|
+ {
|
|
|
+ var arr = new double[1];
|
|
|
+ return Imc60.IMC_GetAxEncVel(_card, (short)axis, arr, 1) == Imc60.EXE_SUCCESS ? arr[0] : 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ internal short ReadHomeStatus(int axis)
|
|
|
+ {
|
|
|
+ short sts = 0;
|
|
|
+ Imc60.IMC_GetHomingStatus(_card, (short)axis, ref sts);
|
|
|
+ return sts;
|
|
|
+ }
|
|
|
+
|
|
|
+ internal void FinishHome(int axis)
|
|
|
+ {
|
|
|
+ Imc60.IMC_FinishHoming(_card, (short)axis);
|
|
|
+ }
|
|
|
+
|
|
|
+ internal void RefreshHomeFlag(int axis)
|
|
|
{
|
|
|
if (!IsConnected) return;
|
|
|
- Imc60.IMC_CloseCard((short)_cfg.BoardIndex);
|
|
|
+ if (_hwAxes.TryGetValue(axis, out var hw))
|
|
|
+ hw.PollHome(this);
|
|
|
+ }
|
|
|
+
|
|
|
+ private bool PrepareCrd(double velocity)
|
|
|
+ {
|
|
|
+ double acc = Math.Max(100, Math.Abs(velocity) * 5);
|
|
|
+ if (!Ok(Imc60.IMC_CrdClrData(_card, 0), "清空插补缓冲")) return false;
|
|
|
+ if (!Ok(Imc60.IMC_CrdSetIncMode(_card, 0, 0), "插补绝对模式")) return false;
|
|
|
+ if (!Ok(Imc60.IMC_CrdSetTrajVel(_card, 0, velocity), "插补速度")) return false;
|
|
|
+ return Ok(Imc60.IMC_CrdSetTrajAccAndDec(_card, 0, acc, acc), "插补加减速");
|
|
|
+ }
|
|
|
+
|
|
|
+ private bool EnsureCrd(int dim)
|
|
|
+ {
|
|
|
+ dim = Math.Max(2, Math.Min(3, dim));
|
|
|
+ if (_hwAxes.Count < dim) return false;
|
|
|
+ if (_crdReady && _crdDim == dim) return true;
|
|
|
+ if (_crdReady)
|
|
|
+ {
|
|
|
+ Imc60.IMC_CrdStop(_card, 0, StopAbrupt);
|
|
|
+ Imc60.IMC_CrdDeleteMtSys(_card, 0);
|
|
|
+ _crdReady = false;
|
|
|
+ }
|
|
|
+
|
|
|
+ var mask = new short[3];
|
|
|
+ mask[0] = 0;
|
|
|
+ mask[1] = 1;
|
|
|
+ mask[2] = (short)(dim > 2 ? 2 : -1);
|
|
|
+ var hw = _hwAxes.Values.FirstOrDefault();
|
|
|
+ double eStopDec = Math.Max(1000, hw != null ? hw.Deceleration * 4 : 1000);
|
|
|
+ if (!Ok(Imc60.IMC_CrdSetMtSys(_card, 0, mask, 100, eStopDec), "建立插补坐标系"))
|
|
|
+ return false;
|
|
|
+ _crdDim = dim;
|
|
|
+ _crdReady = true;
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+
|
|
|
+ public void ReloadFromConfig()
|
|
|
+ {
|
|
|
+ lock (_sync)
|
|
|
+ {
|
|
|
+ RebuildAxes();
|
|
|
+ if (IsConnected)
|
|
|
+ {
|
|
|
+ ApplyAxisBonds();
|
|
|
+ ApplySafetyLimits();
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ private void ApplyAxisBonds()
|
|
|
+ {
|
|
|
+ var cfgs = MotionAxisCatalog.GetEffective(_cfg);
|
|
|
+ foreach (var cfg in cfgs)
|
|
|
+ {
|
|
|
+ short axType;
|
|
|
+ short ch = (short)Math.Max(0, cfg.OutputChannel);
|
|
|
+ switch (cfg.Kind)
|
|
|
+ {
|
|
|
+ case MotionAxisKind.Physical:
|
|
|
+ axType = 1;
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ axType = -1;
|
|
|
+ ch = 0;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ Ok(Imc60.IMC_SetAxBondCfg(_card, (short)cfg.AxisNo, axType, ch),
|
|
|
+ $"轴{cfg.AxisNo}绑定({cfg.KindText})");
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ internal MotionAxisConfig GetAxisConfig(int axisNo)
|
|
|
+ {
|
|
|
+ if (_axisCfgs.TryGetValue(axisNo, out var cfg) && cfg != null)
|
|
|
+ return cfg;
|
|
|
+ return new MotionAxisConfig { AxisNo = axisNo };
|
|
|
+ }
|
|
|
+
|
|
|
+ internal double ClampAxisPosition(int axis, double position)
|
|
|
+ => GetAxisConfig(axis).ClampPosition(position);
|
|
|
+
|
|
|
+ internal double ClampAxisVelocity(int axis, double velocity)
|
|
|
+ => GetAxisConfig(axis).ClampVelocity(velocity);
|
|
|
+
|
|
|
+ internal double ClampAxisJog(int axis, double velocity, double currentPosition)
|
|
|
+ => GetAxisConfig(axis).ClampJog(velocity, currentPosition);
|
|
|
+
|
|
|
+ private double ClampInterpVelocity(double velocity, int dim)
|
|
|
+ {
|
|
|
+ velocity = Math.Abs(velocity);
|
|
|
+ int n = Math.Max(1, dim);
|
|
|
+ for (int i = 0; i < n; i++)
|
|
|
+ velocity = Math.Abs(ClampAxisVelocity(i, velocity));
|
|
|
+ return velocity;
|
|
|
+ }
|
|
|
+
|
|
|
+ private void ApplySafetyLimits()
|
|
|
+ {
|
|
|
+ foreach (var cfg in MotionAxisCatalog.GetEffective(_cfg))
|
|
|
+ {
|
|
|
+ short en = (short)(cfg.SoftLimitEnabled ? 1 : 0);
|
|
|
+ if (cfg.SoftLimitEnabled)
|
|
|
+ {
|
|
|
+ int pos = (int)Math.Round(Math.Max(cfg.SoftLimitNeg, cfg.SoftLimitPos));
|
|
|
+ int neg = (int)Math.Round(Math.Min(cfg.SoftLimitNeg, cfg.SoftLimitPos));
|
|
|
+ Ok(Imc60.IMC_SetAxSoftLimit(_card, (short)cfg.AxisNo, pos, neg),
|
|
|
+ $"轴{cfg.AxisNo}软限位");
|
|
|
+ }
|
|
|
+ Ok(Imc60.IMC_SetAxSoftLmtsCheck(_card, (short)cfg.AxisNo, en),
|
|
|
+ $"轴{cfg.AxisNo}软限位检查");
|
|
|
+
|
|
|
+ if (cfg.MaxVelocity <= 0) continue;
|
|
|
+ var para = new Imc60.TMtPara
|
|
|
+ {
|
|
|
+ bgVel = 0,
|
|
|
+ maxVel = cfg.MaxVelocity,
|
|
|
+ maxAcc = Math.Max(1, cfg.Acceleration),
|
|
|
+ maxDec = Math.Max(1, cfg.Deceleration),
|
|
|
+ maxJerk = Math.Max(1, cfg.Jerk),
|
|
|
+ stopDec = Math.Max(1, cfg.Deceleration),
|
|
|
+ eStopDec = Math.Max(1, cfg.Deceleration * 4)
|
|
|
+ };
|
|
|
+ Ok(Imc60.IMC_SetAxMaxMtPara(_card, (short)cfg.AxisNo, ref para),
|
|
|
+ $"轴{cfg.AxisNo}最高速度");
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ private void RebuildAxes()
|
|
|
+ {
|
|
|
+ _follow.Clear();
|
|
|
+ var oldHw = _hwAxes;
|
|
|
+ var cfgs = MotionAxisCatalog.GetEffective(_cfg);
|
|
|
+ _axisCfgs = new Dictionary<int, MotionAxisConfig>();
|
|
|
+ _hwAxes = new Dictionary<int, InovanceAxis>();
|
|
|
+ _axisMap = new Dictionary<int, IAxis>();
|
|
|
+ var view = new List<IAxis>(cfgs.Count);
|
|
|
+
|
|
|
+ foreach (var cfg in cfgs)
|
|
|
+ {
|
|
|
+ var axis = new InovanceAxis(this, cfg.AxisNo)
|
|
|
+ {
|
|
|
+ Name = string.IsNullOrWhiteSpace(cfg.Name) ? $"Axis-{cfg.AxisNo}" : cfg.Name,
|
|
|
+ Velocity = cfg.Velocity,
|
|
|
+ Acceleration = cfg.Acceleration,
|
|
|
+ Deceleration = cfg.Deceleration,
|
|
|
+ Jerk = cfg.Jerk
|
|
|
+ };
|
|
|
+ if (oldHw != null && oldHw.TryGetValue(cfg.AxisNo, out var prev))
|
|
|
+ {
|
|
|
+ axis.Name = string.IsNullOrWhiteSpace(cfg.Name) ? prev.Name : cfg.Name;
|
|
|
+ axis.Velocity = cfg.Velocity > 0 ? cfg.Velocity : prev.Velocity;
|
|
|
+ axis.Acceleration = cfg.Acceleration > 0 ? cfg.Acceleration : prev.Acceleration;
|
|
|
+ axis.Deceleration = cfg.Deceleration > 0 ? cfg.Deceleration : prev.Deceleration;
|
|
|
+ axis.Jerk = cfg.Jerk > 0 ? cfg.Jerk : prev.Jerk;
|
|
|
+ }
|
|
|
+ _hwAxes[cfg.AxisNo] = axis;
|
|
|
+ _axisMap[cfg.AxisNo] = axis;
|
|
|
+ _axisCfgs[cfg.AxisNo] = cfg;
|
|
|
+ }
|
|
|
+
|
|
|
+ foreach (var cfg in cfgs)
|
|
|
+ {
|
|
|
+ if (cfg.IsGrouped && _hwAxes.TryGetValue(cfg.AxisNo, out var master))
|
|
|
+ {
|
|
|
+ var combined = new CombinedMotionAxis(this, cfg, master, no =>
|
|
|
+ _hwAxes.TryGetValue(no, out var a) ? a : null);
|
|
|
+ _axisMap[cfg.AxisNo] = combined;
|
|
|
+ view.Add(combined);
|
|
|
+ }
|
|
|
+ else if (_axisMap.TryGetValue(cfg.AxisNo, out var hw))
|
|
|
+ {
|
|
|
+ view.Add(hw);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ _axes = view.ToArray();
|
|
|
+ _axisCount = _axes.Length;
|
|
|
+ }
|
|
|
+
|
|
|
+ private bool TryCombined(int axis, out CombinedMotionAxis combined)
|
|
|
+ {
|
|
|
+ combined = null;
|
|
|
+ if (_axisMap.TryGetValue(axis, out var a) && a is CombinedMotionAxis c)
|
|
|
+ {
|
|
|
+ combined = c;
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ private bool IsHardware(int axis) => _hwAxes.ContainsKey(axis);
|
|
|
+
|
|
|
+ private void ParseConfigPaths(out string deviceCfg, out string systemCfg)
|
|
|
+ {
|
|
|
+ deviceCfg = "";
|
|
|
+ systemCfg = "";
|
|
|
+ var raw = _cfg.ConnectionString?.Trim();
|
|
|
+ if (string.IsNullOrEmpty(raw)) return;
|
|
|
+ var parts = raw.Split(new[] { '|', ';' }, StringSplitOptions.RemoveEmptyEntries);
|
|
|
+ if (parts.Length > 0) deviceCfg = ResolvePath(parts[0].Trim());
|
|
|
+ if (parts.Length > 1) systemCfg = ResolvePath(parts[1].Trim());
|
|
|
+ }
|
|
|
+
|
|
|
+ private static string ResolvePath(string path)
|
|
|
+ {
|
|
|
+ if (string.IsNullOrWhiteSpace(path)) return "";
|
|
|
+ path = path.Trim().Trim('"');
|
|
|
+ if (File.Exists(path)) return Path.GetFullPath(path);
|
|
|
+ var baseDir = AppDomain.CurrentDomain.BaseDirectory ?? "";
|
|
|
+ var combined = Path.Combine(baseDir, path);
|
|
|
+ if (File.Exists(combined)) return Path.GetFullPath(combined);
|
|
|
+ var inConfig = Path.Combine(baseDir, "Config", path);
|
|
|
+ if (File.Exists(inConfig)) return Path.GetFullPath(inConfig);
|
|
|
+ return Path.IsPathRooted(path) ? path : Path.GetFullPath(combined);
|
|
|
+ }
|
|
|
+
|
|
|
+ private static short MapHomeMethod(int mode)
|
|
|
+ {
|
|
|
+ if (mode > 3) return (short)mode;
|
|
|
+ switch ((HomeMode)mode)
|
|
|
+ {
|
|
|
+ case HomeMode.LimitSwitch: return (short)Imc60.HOME_NLIMT;
|
|
|
+ case HomeMode.HomeSwitch: return (short)Imc60.HOME_NHOME_FEDGE;
|
|
|
+ case HomeMode.ZPhase: return (short)Imc60.HOME_NEGZINDEX;
|
|
|
+ default: return (short)Imc60.HOME_NLIMT_ZINDEX;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ private static uint ToUInt(double v) => (uint)Math.Max(1, Math.Min(uint.MaxValue, v));
|
|
|
+
|
|
|
+ private static bool Has(int sts, uint bit) => (sts & (int)bit) != 0;
|
|
|
+
|
|
|
+ private bool IsValidAxis(int axis) => _axisMap.ContainsKey(axis);
|
|
|
+
|
|
|
+ private bool Ok(uint ret, string what)
|
|
|
+ {
|
|
|
+ if (ret == Imc60.EXE_SUCCESS) return true;
|
|
|
+ RaiseError($"{what}失败 (0x{ret:X8})");
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ private bool FailOpen()
|
|
|
+ {
|
|
|
+ try { Imc60.IMC_DelEcatComm(_card); } catch { }
|
|
|
+ try { Imc60.IMC_CloseCard(_card); } catch { }
|
|
|
IsConnected = false;
|
|
|
- ConnectionStateChanged?.Invoke(this, new MotionConnectionStateChangedEventArgs(false));
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ private void RaiseError(string message)
|
|
|
+ {
|
|
|
+ try { AppLogger.Error(message, null, nameof(InovanceMotionCard)); } catch { }
|
|
|
+ ErrorOccurred?.Invoke(this, new MotionErrorEventArgs(message));
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>汇川卡单轴门面:实时读 SDK 状态,指令转发到卡。</summary>
|
|
|
+ public class InovanceAxis : IAxis
|
|
|
+ {
|
|
|
+ private readonly InovanceMotionCard _card;
|
|
|
+ private bool _homed;
|
|
|
+ private bool _homing;
|
|
|
+ private bool _jogging;
|
|
|
+ private AxisState _lastState = AxisState.Off;
|
|
|
+
|
|
|
+ public int AxisNo { get; }
|
|
|
+ public string Name { get; set; }
|
|
|
+ public double Velocity { get; set; } = 100;
|
|
|
+ public double Acceleration { get; set; } = 500;
|
|
|
+ public double Deceleration { get; set; } = 500;
|
|
|
+ public double Jerk { get; set; } = 1000;
|
|
|
+
|
|
|
+ public AxisState State
|
|
|
+ {
|
|
|
+ get
|
|
|
+ {
|
|
|
+ if (!_card.IsConnected) return AxisState.Off;
|
|
|
+ int sts = _card.ReadSts(AxisNo);
|
|
|
+ if ((sts & (int)Imc60.AX_ALARM_BIT) != 0 || (sts & (int)Imc60.AX_UNLINK_BIT) != 0)
|
|
|
+ return AxisState.Error;
|
|
|
+ if ((sts & (int)Imc60.AX_SVON_BIT) == 0) return AxisState.Off;
|
|
|
+ if (_homing) return AxisState.Homing;
|
|
|
+ if (_jogging) return AxisState.ContinuousMotion;
|
|
|
+ if ((sts & (int)Imc60.AX_BUSY_BIT) != 0) return AxisState.DiscreteMotion;
|
|
|
+ return AxisState.StandStill;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ public bool IsEnabled => _card.IsConnected && (_card.ReadSts(AxisNo) & (int)Imc60.AX_SVON_BIT) != 0;
|
|
|
+ public bool IsMoving => _card.IsConnected && ((_card.ReadSts(AxisNo) & (int)Imc60.AX_BUSY_BIT) != 0 || _jogging || _homing);
|
|
|
+ public bool IsHomed => _homed;
|
|
|
+ public double CommandPosition => _card.IsConnected ? _card.ReadPrfPos(AxisNo) : 0;
|
|
|
+ public double ActualPosition => _card.IsConnected ? _card.ReadEncPos(AxisNo) : 0;
|
|
|
+ public double ActualVelocity => _card.IsConnected ? _card.ReadEncVel(AxisNo) : 0;
|
|
|
+
|
|
|
+ public event EventHandler<AxisMotionEventArgs> MotionStarted;
|
|
|
+ public event EventHandler<AxisMotionEventArgs> MotionCompleted;
|
|
|
+ public event EventHandler<AxisStateChangedEventArgs> StateChanged;
|
|
|
+ public event EventHandler<AxisErrorEventArgs> ErrorOccurred;
|
|
|
+
|
|
|
+ public InovanceAxis(InovanceMotionCard card, int axisNo)
|
|
|
+ {
|
|
|
+ _card = card;
|
|
|
+ AxisNo = axisNo;
|
|
|
+ Name = $"Axis-{axisNo}";
|
|
|
+ }
|
|
|
+
|
|
|
+ internal void MarkHoming()
|
|
|
+ {
|
|
|
+ _homing = true;
|
|
|
+ _homed = false;
|
|
|
+ NotifyState();
|
|
|
+ }
|
|
|
+
|
|
|
+ internal void MarkJogging(bool on)
|
|
|
+ {
|
|
|
+ _jogging = on;
|
|
|
+ NotifyState();
|
|
|
+ }
|
|
|
+
|
|
|
+ internal void AbortHome()
|
|
|
+ {
|
|
|
+ if (!_homing) return;
|
|
|
+ try { _card.FinishHome(AxisNo); } catch { }
|
|
|
+ _homing = false;
|
|
|
+ NotifyState();
|
|
|
+ }
|
|
|
+
|
|
|
+ internal void PollHome(InovanceMotionCard card)
|
|
|
+ {
|
|
|
+ if (!_homing) return;
|
|
|
+ short hs = card.ReadHomeStatus(AxisNo);
|
|
|
+ if (hs == Imc60.HOME_IN_PROGRESS || hs == Imc60.HOME_INTERRUPTED_OR_NOT_START)
|
|
|
+ return;
|
|
|
+
|
|
|
+ if (hs == Imc60.HOME_SUCESS || hs == Imc60.HOME_ATTAINED_BUT_NOT_REACH)
|
|
|
+ {
|
|
|
+ card.FinishHome(AxisNo);
|
|
|
+ _homing = false;
|
|
|
+ _homed = true;
|
|
|
+ NotifyState();
|
|
|
+ MotionCompleted?.Invoke(this, new AxisMotionEventArgs(AxisNo, 0, ActualPosition));
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ card.FinishHome(AxisNo);
|
|
|
+ _homing = false;
|
|
|
+ NotifyState();
|
|
|
+ OnError($"回零失败 (状态 {hs})");
|
|
|
+ }
|
|
|
+
|
|
|
+ public void Enable()
|
|
|
+ {
|
|
|
+ if (!_card.AllowEnable(out var reason))
|
|
|
+ {
|
|
|
+ ErrorOccurred?.Invoke(this, new AxisErrorEventArgs(AxisNo, reason));
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if (!_card.SetEnable(AxisNo, true))
|
|
|
+ ErrorOccurred?.Invoke(this, new AxisErrorEventArgs(AxisNo, "使能失败"));
|
|
|
+ NotifyState();
|
|
|
+ }
|
|
|
+
|
|
|
+ public void Disable()
|
|
|
+ {
|
|
|
+ _jogging = false;
|
|
|
+ AbortHome();
|
|
|
+ _card.Stop(AxisNo);
|
|
|
+ if (!_card.SetEnable(AxisNo, false))
|
|
|
+ ErrorOccurred?.Invoke(this, new AxisErrorEventArgs(AxisNo, "下使能失败"));
|
|
|
+ NotifyState();
|
|
|
}
|
|
|
|
|
|
- public bool SetEnable(int axis, bool enable) => throw new NotImplementedException();
|
|
|
- public bool Home(int axis, int mode = 0) => throw new NotImplementedException();
|
|
|
- public bool IsHomed(int axis) => throw new NotImplementedException();
|
|
|
- public bool MoveAbsolute(int axis, double position, double velocity) => throw new NotImplementedException();
|
|
|
- public bool MoveRelative(int axis, double distance, double velocity) => throw new NotImplementedException();
|
|
|
- public bool JogStart(int axis, int direction, double velocity) => throw new NotImplementedException();
|
|
|
- public bool JogStop(int axis) => throw new NotImplementedException();
|
|
|
- public bool Stop(int axis = -1) => throw new NotImplementedException();
|
|
|
- public AxisStatus GetAxisStatus(int axis) => throw new NotImplementedException();
|
|
|
- public List<AxisStatus> GetAllAxisStatus() => throw new NotImplementedException();
|
|
|
- public IAxis GetAxis(int axisNo) => throw new NotImplementedException();
|
|
|
- public IList<IAxis> GetAxes() => throw new NotImplementedException();
|
|
|
- public bool MoveLinear(double[] targetPositions, double velocity) => throw new NotImplementedException();
|
|
|
- public bool MoveArc(double[] centerPoint, double[] targetPositions, double velocity) => throw new NotImplementedException();
|
|
|
- public bool EmergencyStop() => throw new NotImplementedException();
|
|
|
- public bool ReadDI(int portIndex) => throw new NotImplementedException();
|
|
|
- public bool WriteDO(int portIndex, bool value) => throw new NotImplementedException();
|
|
|
+ public void MoveTo(double position)
|
|
|
+ {
|
|
|
+ if (!_card.IsConnected) { OnError("运动卡未连接"); return; }
|
|
|
+ if (!_card.AllowMotion(out var reason)) { OnError(reason); return; }
|
|
|
+ if (!IsEnabled) { OnError("轴未使能,无法运动"); return; }
|
|
|
+ if (_card.MoveAbsolute(AxisNo, position, Velocity))
|
|
|
+ {
|
|
|
+ NotifyState();
|
|
|
+ MotionStarted?.Invoke(this, new AxisMotionEventArgs(AxisNo, position, ActualPosition));
|
|
|
+ }
|
|
|
+ else
|
|
|
+ OnError("绝对定位失败");
|
|
|
+ }
|
|
|
+
|
|
|
+ public void MoveBy(double distance)
|
|
|
+ {
|
|
|
+ if (!_card.IsConnected) { OnError("运动卡未连接"); return; }
|
|
|
+ if (!_card.AllowMotion(out var reason)) { OnError(reason); return; }
|
|
|
+ if (!IsEnabled) { OnError("轴未使能,无法运动"); return; }
|
|
|
+ if (_card.MoveRelative(AxisNo, distance, Velocity))
|
|
|
+ {
|
|
|
+ NotifyState();
|
|
|
+ MotionStarted?.Invoke(this, new AxisMotionEventArgs(AxisNo, ActualPosition + distance, ActualPosition));
|
|
|
+ }
|
|
|
+ else
|
|
|
+ OnError("相对定位失败");
|
|
|
+ }
|
|
|
+
|
|
|
+ public void Jog(double velocity)
|
|
|
+ {
|
|
|
+ if (!_card.IsConnected) { OnError("运动卡未连接"); return; }
|
|
|
+ if (!_card.AllowMotion(out var reason)) { OnError(reason); return; }
|
|
|
+ if (!IsEnabled) { OnError("轴未使能,无法点动"); return; }
|
|
|
+ int dir = velocity >= 0 ? 1 : -1;
|
|
|
+ if (_card.JogStart(AxisNo, dir, Math.Abs(velocity)))
|
|
|
+ {
|
|
|
+ NotifyState();
|
|
|
+ MotionStarted?.Invoke(this, new AxisMotionEventArgs(AxisNo, double.NaN, ActualPosition));
|
|
|
+ }
|
|
|
+ else
|
|
|
+ OnError("点动失败");
|
|
|
+ }
|
|
|
+
|
|
|
+ public void Stop()
|
|
|
+ {
|
|
|
+ AbortHome();
|
|
|
+ _jogging = false;
|
|
|
+ _card.Stop(AxisNo);
|
|
|
+ NotifyState();
|
|
|
+ MotionCompleted?.Invoke(this, new AxisMotionEventArgs(AxisNo, CommandPosition, ActualPosition));
|
|
|
+ }
|
|
|
+
|
|
|
+ public void Home(HomeMode mode = HomeMode.Default)
|
|
|
+ {
|
|
|
+ if (!_card.IsConnected) { OnError("运动卡未连接"); return; }
|
|
|
+ if (!_card.AllowMotion(out var reason)) { OnError(reason); return; }
|
|
|
+ if (!IsEnabled) { OnError("轴未使能,无法回零"); return; }
|
|
|
+ if (_card.Home(AxisNo, (int)mode))
|
|
|
+ {
|
|
|
+ NotifyState();
|
|
|
+ MotionStarted?.Invoke(this, new AxisMotionEventArgs(AxisNo, 0, ActualPosition));
|
|
|
+ }
|
|
|
+ else
|
|
|
+ OnError("回零失败");
|
|
|
+ }
|
|
|
+
|
|
|
+ public void SetPosition(double position)
|
|
|
+ {
|
|
|
+ if (!_card.SetCurrentPosition(AxisNo, position))
|
|
|
+ OnError("设置当前位置失败");
|
|
|
+ }
|
|
|
+
|
|
|
+ public void SetMotionParams(MotionParams parameters)
|
|
|
+ {
|
|
|
+ if (parameters == null) return;
|
|
|
+ Velocity = parameters.Velocity;
|
|
|
+ Acceleration = parameters.Acceleration;
|
|
|
+ Deceleration = parameters.Deceleration;
|
|
|
+ Jerk = parameters.Jerk;
|
|
|
+ _card.ApplyMovePara(AxisNo, Velocity);
|
|
|
+ }
|
|
|
+
|
|
|
+ private void OnError(string message) => ErrorOccurred?.Invoke(this, new AxisErrorEventArgs(AxisNo, message));
|
|
|
+
|
|
|
+ internal void SyncState() => NotifyState();
|
|
|
+
|
|
|
+ private void NotifyState()
|
|
|
+ {
|
|
|
+ var now = State;
|
|
|
+ if (now == _lastState) return;
|
|
|
+ var old = _lastState;
|
|
|
+ _lastState = now;
|
|
|
+ StateChanged?.Invoke(this, new AxisStateChangedEventArgs(AxisNo, old, now));
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
|