using Bro.Common.Base;
|
using Bro.Common.Helper;
|
using Bro.Common.Interface;
|
using Bro.Common.Model;
|
using System;
|
using System.Collections.Concurrent;
|
using System.Collections.Generic;
|
using System.Collections.ObjectModel;
|
using System.Drawing;
|
using System.Linq;
|
using System.Text;
|
using System.Threading;
|
using System.Threading.Tasks;
|
|
namespace Bro.Device.GTSCard
|
{
|
[Device("GTSCard", "固高板卡", EnumHelper.DeviceAttributeType.Device)]
|
public class GTSCardDriver : DeviceBase, IMonitor, IMotion
|
{
|
public event Action<DateTime, string, IDevice, MonitorSet> OnMonitorInvoke;
|
public event Action<DateTime, IDevice, WarningSet> OnMonitorAlarm;
|
|
public delegate bool OnAxisStartToCheckDelegate(int axisIndex, int startPosition, int endPosition);
|
// 异常事件
|
public Action<Exception> OnExceptionRaised;
|
|
public GTSCardInitialConfig IConfig
|
{
|
get
|
{
|
return InitialConfig as GTSCardInitialConfig;
|
}
|
}
|
|
static Dictionary<int, object> axisMoveLockDict = new Dictionary<int, object>();
|
|
/// <summary>
|
/// 轴运动开始时的检测，true:有冲突 不可继续执行 false：无冲突,可继续执行
|
/// </summary>
|
public event OnAxisStartToCheckDelegate OnAxisStartToCheckConfliction;
|
/// <summary>
|
/// 暂停（线程同步事件）
|
/// </summary>
|
Dictionary<int, ManualResetEvent> axisImmediatePauseHandleDict = new Dictionary<int, ManualResetEvent>();
|
Dictionary<int, CancellationTokenSource> axisMoveCancelDict = new Dictionary<int, CancellationTokenSource>();
|
/// <summary>
|
/// 运行过程中的线程等待
|
/// </summary>
|
private Dictionary<int, ManualResetEvent> runningEventDic = new Dictionary<int, ManualResetEvent>();
|
private Dictionary<int, AutoResetEvent> axisMovingHandleDict = new Dictionary<int, AutoResetEvent>();
|
private ConcurrentDictionary<int, int> axisDestination = new ConcurrentDictionary<int, int>();
|
|
private ObservableCollection<int> _commandAxisList = new ObservableCollection<int>();
|
public Action<bool> CommandAxisCountChangedAction = null;
|
private Dictionary<int, VelocityPara> velIndexDict = new Dictionary<int, VelocityPara>();
|
ManualResetEvent _pauseHandle = new ManualResetEvent(true);
|
|
static object lockObj = new object();
|
static object _commandAxisLock = new object();
|
/// <summary>
|
/// 是否复位标志
|
/// </summary>
|
bool _isResetting = false;
|
|
public void SetResetFlag(bool isReset)
|
{
|
_isResetting = isReset;
|
}
|
|
public List<AxisInfo> GetCurrentAxisInfo(params string[] axisName)
|
{
|
throw new NotImplementedException();
|
}
|
|
#region DeviceBase
|
|
protected override void Init()
|
{
|
InitialMotionCard((short)IConfig.CardNum, IConfig.InitialConfigFilePath);
|
|
axisMoveLockDict = IConfig.AxisSettings.ToDictionary(a => a.AxisIndex, a => new object());
|
runningEventDic = IConfig.AxisSettings.ToDictionary(a => a.AxisIndex, a => new ManualResetEvent(false));
|
axisMovingHandleDict = IConfig.AxisSettings.ToDictionary(a => a.AxisIndex, a => new AutoResetEvent(true));
|
axisImmediatePauseHandleDict = IConfig.AxisSettings.ToDictionary(a => a.AxisIndex, a => new ManualResetEvent(true));
|
axisMoveCancelDict = IConfig.AxisSettings.ToDictionary(a => a.AxisIndex, a => new CancellationTokenSource());
|
|
axisMoveCancelDict.Values.ToList().ForEach(c =>
|
{
|
c = new CancellationTokenSource();
|
});
|
|
_commandAxisList.CollectionChanged -= CommandAxisList_CollectionChanged;
|
_commandAxisList.CollectionChanged += CommandAxisList_CollectionChanged;
|
}
|
|
private void CommandAxisList_CollectionChanged(object sender, System.Collections.Specialized.NotifyCollectionChangedEventArgs e)
|
{
|
CommandAxisCountChangedAction?.Invoke(_commandAxisList.Count > 0);
|
}
|
|
protected override void Pause()
|
{
|
throw new NotImplementedException();
|
}
|
|
protected override void Resume()
|
{
|
throw new NotImplementedException();
|
}
|
|
protected override void Start()
|
{
|
throw new NotImplementedException();
|
}
|
|
protected override void Stop()
|
{
|
throw new NotImplementedException();
|
}
|
#endregion
|
|
#region GTSCard
|
|
public void ClearPosition(short cardNum, short axisNum)
|
{
|
int ret = GTSCardAPI.GT_SetPos(cardNum, axisNum, 0);
|
}
|
|
/// <summary>
|
/// Load Motion Card parameter from file
|
/// </summary>
|
/// <param name="fileName">Invalid Parameter</param>
|
/// <returns></returns>
|
public void InitialMotionCard(short cardNum, string fileName)
|
{
|
var res = GTSCardAPI.GT_LoadConfig(cardNum, fileName);
|
if (res != GTSCardAPI.ResultSuccess)
|
{
|
throw new Exception("板卡载入配置文件异常，错误码：" + res);
|
}
|
|
}
|
|
/// <summary>
|
/// 单个轴 点位到点位运动（异步）
|
/// </summary>
|
/// <param name="item">运动对象</param>
|
/// <returns></returns>
|
public async Task SingleAxisMovingAsync(MovingOption item)
|
{
|
await Task.Run(() =>
|
{
|
SingleAxisMoving(item);
|
});
|
}
|
|
/// <summary>
|
/// 单个轴 点位到点位运动（异步）
|
/// </summary>
|
/// <param name="item">运动对象</param>
|
public void SingleAxisMoving(MovingOption item)
|
{
|
if (IConfig.AxisSettings.FirstOrDefault(a => a.AxisIndex == item.AxisIndex)?.IsAxisEnabled ?? false)
|
{
|
axisImmediatePauseHandleDict[item.AxisIndex].WaitOne();
|
VelocityPara vel = new VelocityPara();
|
if (item.VelocityPara.Velocity != 0)
|
{
|
velIndexDict[item.AxisIndex] = vel = item.VelocityPara;
|
}
|
else
|
{
|
vel = velIndexDict[item.AxisIndex];
|
}
|
|
string _position = "";
|
string motionType = item.MoveMode == EnumHelper.MotorMoveMode.Normal ? (item.IsAbsolute ? "Abs" : "Rel") : item.MoveMode.ToString();
|
|
_position = $"{DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss.fff")},{item.AxisIndex},{motionType},{GetCmdOrPosition(item.AxisIndex, 0).ToString()},{GetCmdOrPosition(item.AxisIndex, 1).ToString()},{item.Destination},";
|
|
lock (axisMoveLockDict[item.AxisIndex])
|
{
|
Task.Run(() =>
|
{
|
lock (_commandAxisLock)
|
{
|
try
|
{
|
if (!_commandAxisList.Contains(item.AxisIndex))
|
{
|
_commandAxisList.Add(item.AxisIndex);
|
}
|
}
|
catch (Exception)
|
{
|
}
|
}
|
});
|
|
switch (item.MoveMode)
|
{
|
case EnumHelper.MotorMoveMode.Normal:
|
{
|
if (_isResetting)
|
{
|
LogAsync(DateTime.Now, "复位中启动运动异常", item.AxisIndex + "启动运动异常");
|
return;
|
}
|
|
SetAxisParam(item.AxisIndex, vel);
|
if (item.IsAbsolute)
|
{
|
MoveAbs(item.AxisIndex, item.Destination, (int)(vel.Velocity * IConfig.AxisVelocityRatio));
|
}
|
else
|
{
|
MoveRel(item.AxisIndex, item.Destination, (int)(vel.Velocity * IConfig.AxisVelocityRatio));
|
}
|
}
|
break;
|
case EnumHelper.MotorMoveMode.FindOri:
|
{
|
AxisSetting setting = IConfig.AxisSettings.FirstOrDefault(u => u.AxisIndex == item.AxisIndex);
|
StartHoming(item.AxisIndex, setting.HomeMode, setting.IsHomePositive ? 1 : 0, item.VelocityPara.Dec, item.VelocityPara.Acc, item.VelocityPara.Velocity);
|
}
|
break;
|
}
|
|
Task.Run(() =>
|
{
|
lock (_commandAxisLock)
|
{
|
try
|
{
|
_commandAxisList.Remove(item.AxisIndex);
|
}
|
catch (Exception)
|
{
|
}
|
}
|
});
|
}
|
|
_position += $"{GetCmdOrPosition(item.AxisIndex, 0).ToString()},";
|
_position += $"{DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss.fff")}";
|
LogAsync(DateTime.Now, "", _position);
|
}
|
}
|
|
public int GetCmdOrPosition(int axisNum, int flag = 0)
|
{
|
int position = 0;
|
|
if (flag == 0)
|
{
|
GetPosition(axisNum, ref position);
|
}
|
else
|
{
|
GetCmdPosition(axisNum, ref position);
|
}
|
|
return position;
|
}
|
|
/// <summary>
|
///Get single Axis Feedback position
|
/// </summary>
|
/// <param name="axisNum">Axis number</param>
|
/// <param name="nPosition">Feedback/Encorde position </param>
|
/// <returns></returns>
|
public void GetPosition(int axisNum, ref int nPosition)
|
{
|
lock (lockObj)
|
{
|
double prfpos = 0; uint pclock = 0;
|
var ret = GTSCardAPI.GT_GetPrfPos((short)IConfig.CardNum, (short)axisNum, out prfpos, 1, out pclock);
|
if (ret != GTSCardAPI.ResultSuccess)
|
{
|
throw new Exception("轴" + axisNum + "获取当前位置异常，错误码：" + ret);
|
}
|
nPosition = prfpos / IConfig.AxisVelocityRatio;
|
}
|
}
|
|
public int GetPosition(int axisNum)
|
{
|
int position = 0;
|
|
if (!IConfig.AxisSettings.FirstOrDefault(u => u.AxisIndex == axisNum).IsUseCmmdPosition)
|
{
|
GetPosition(axisNum, ref position);
|
}
|
else
|
{
|
GetCmdPosition(axisNum, ref position);
|
}
|
|
return position;
|
}
|
|
/// <summary>
|
///Get single Axis Command position
|
/// </summary>
|
/// <param name="axisNum">Axis number</param>
|
/// <param name="nPosition">Command position </param>
|
/// <returns></returns>
|
public void GetCmdPosition(int axisNum, ref int nPosition)
|
{
|
var ret = GTSCardAPI.APS_get_command(axisNum, ref nPosition);
|
if (ret != (Int32)GTSCardParameter.ResultSuccess)
|
{
|
throw new Exception("轴" + axisNum + "获取当前位置异常，错误码：" + ret);
|
}
|
nPosition = prfpos / IConfig.AxisVelocityRatio;
|
}
|
|
/// <summary>
|
/// Set AxisParam
|
/// </summary>
|
/// <param name="axisNo"></param>
|
/// <param name="param"></param>
|
/// <returns></returns>
|
public void SetAxisParam(int axisNum, VelocityPara param)
|
{
|
int ret = 0;
|
GTSCardAPI.TTrapPrm trapprm;
|
GTSCardAPI.GT_PrfTrap((short)IConfig.CardNum, (short)axisNum);
|
GTSCardAPI.GT_GetTrapPrm((short)IConfig.CardNum, (short)axisNum, out trapprm);
|
trapprm.smoothTime = 1;
|
if (param.Acc != 0)
|
{
|
trapprm.acc = param.Acc;
|
}
|
if (param.Dec != 0)
|
{
|
trapprm.dec = param.Dec;
|
}
|
ret += GTSCardAPI.GT_SetTrapPrm((short)IConfig.CardNum, (short)axisNum, ref trapprm);
|
ret += GTSCardAPI.GT_SetVel((short)IConfig.CardNum, (short)axisNum, param.Velocity * IConfig.AxisVelocityRatio);
|
|
if (ret != (int)APS_Define.ResultSuccess)
|
{
|
throw new Exception("轴" + axisNum + "设置参数异常，错误码：" + ret);
|
}
|
}
|
|
/// <summary>
|
/// Set Single Axis Do Jog Move
|
/// </summary>
|
/// <param name="axisNum">AxisNo</param>
|
/// <param name="nDirection">Motion Direction 0: Negative, 1: Positive</param>
|
/// <param name="nMaxVel">max velocity</param>
|
/// <returns></returns>
|
public bool StartJog(int axisNum, int nDirection, int velocity)
|
{
|
GTSCardAPI.TJogPrm jogprm = new GTSCardAPI.TJogPrm();
|
short rtn = GTSCardAPI.GT_PrfJog((short)IConfig.CardNum, (short)axisNum);
|
jogprm.acc = 1;
|
jogprm.dec = 1;
|
GTSCardAPI.GT_SetJogPrm((short)IConfig.CardNum, (short)axisNum, ref jogprm);//设置jog运动参数
|
GTSCardAPI.GT_SetVel((short)IConfig.CardNum, (short)axisNum, velocity);//设置目标速度
|
int ret = GTSCardAPI.GT_Update((short)IConfig.CardNum, 1 << (axisNum - 1));//更新轴运动
|
|
if (ret != (int)APS_Define.ResultSuccess)
|
{
|
return false;
|
}
|
return true;
|
}
|
|
/// <summary>
|
/// Set Single Axis Do stop Jog Move
|
/// </summary>
|
/// <param name="axisNum">AxisNo</param>
|
/// <returns></returns>
|
public bool StopJog(int axisNum)
|
{
|
MoveStop();
|
return IsStop((short)IConfig.CardNum, (short)axisNum);
|
}
|
|
/// <summary>
|
/// Set Single Axis Do Rel Move
|
/// </summary>
|
/// <param name="axisNum">AxisNo</param>
|
/// <param name="nDistance">run distance</param>
|
/// <returns></returns>
|
public void MoveRel(int axisNum, int nDistance, int nMaxVel)
|
{
|
try
|
{
|
if (CurrentState == EnumHelper.DeviceState.DSExcept)
|
{
|
LogAsync(DateTime.Now, "板卡异常状态", "轴" + axisNum + "试图异常状态运动");
|
return;
|
}
|
|
if (CurrentState != EnumHelper.DeviceState.DSOpen)
|
{
|
return;
|
}
|
|
int currentPosition = GetPosition(axisNum);
|
|
if (OnAxisStartToCheckConfliction != null && OnAxisStartToCheckConfliction.Invoke(axisNum, currentPosition, currentPosition + nDistance))
|
{
|
return;
|
}
|
|
int ret = GTSCardAPI.GT_SetPos((short)IConfig.CardNum, (short)axisNum, nDistance * IConfig.AxisVelocityRatio);
|
|
ret += GTSCardAPI.GT_Update((short)IConfig.CardNum, 1 << (axisNum - 1));
|
if (ret != (Int32)APS_Define.ResultSuccess)
|
{
|
throw new Exception("轴" + axisNum + "启动相对运动异常，错误码：" + ret);
|
}
|
|
RunFinish(axisNum, false);
|
}
|
catch (Exception ex)
|
{
|
MoveStop();
|
OnExceptionRaised?.Invoke(ex);
|
}
|
}
|
|
/// <summary>
|
/// Set Single Axis Do Absolute Move
|
/// </summary>
|
/// <param name="axisNum">AxisNo</param>
|
/// <param name="nDistance">run distance</param>
|
/// <param name="nMaxVel">max velocity</param>
|
/// <returns></returns>
|
public void MoveAbs(int axisNum, int nPosition, int nMaxVel)
|
{
|
try
|
{
|
ReMove:
|
MoveAbsAsync(axisNum, nPosition);
|
|
var runFinish = Task.Run(() => RunFinish(axisNum), axisMoveCancelDict[axisNum].Token);
|
try
|
{
|
runFinish.Wait(axisMoveCancelDict[axisNum].Token);
|
}
|
catch (OperationCanceledException ex)
|
{
|
goto ReMove;
|
}
|
|
//// 删除记录
|
//if (currentCommandDic.ContainsKey(axisNum))
|
//{
|
// currentCommandDic.TryRemove(axisNum, out int[] temp);
|
//}
|
}
|
catch (Exception ex)
|
{
|
MoveStop();
|
OnExceptionRaised?.Invoke(ex);
|
}
|
}
|
|
static object moveLock = new object();
|
|
/// <summary>
|
/// 绝对运动（异步）
|
/// </summary>
|
/// <param name="axisNum"></param>
|
/// <param name="nPosition"></param>
|
public void MoveAbsAsync(int axisNum, int nPosition)
|
{
|
try
|
{
|
lock (moveLock)
|
{
|
axisImmediatePauseHandleDict[axisNum].WaitOne();
|
_pauseHandle.WaitOne();
|
|
int currentPosition = GetPosition(axisNum);
|
if (OnAxisStartToCheckConfliction != null && OnAxisStartToCheckConfliction.Invoke(axisNum, currentPosition, nPosition))
|
{
|
return;
|
}
|
|
if (_isResetting)
|
{
|
LogAsync(DateTime.Now, "复位过程异常", "轴" + axisNum + "试图在复位过程中运动");
|
throw new Exception("轴" + axisNum + "试图在复位过程中运动");
|
}
|
|
int repeatTime = 30;
|
while (CurrentState != EnumHelper.DeviceState.DSOpen && repeatTime > 0)
|
{
|
Thread.Sleep(10);
|
repeatTime--;
|
}
|
|
if (CurrentState == EnumHelper.DeviceState.DSExcept)
|
{
|
LogAsync(DateTime.Now, "板卡异常状态", "轴" + axisNum + "试图异常状态运动");
|
return;
|
}
|
|
if (CurrentState != EnumHelper.DeviceState.DSOpen)
|
{
|
LogAsync(DateTime.Now, "非正常状态异常", "轴" + axisNum + "试图在非正常状态运动");
|
throw new Exception("轴" + axisNum + "试图在非正常状态运动", null);
|
}
|
|
int ret = 0;
|
repeatTime = 50;
|
do
|
{
|
LogAsync(DateTime.Now, "轴" + axisNum + "启动运动", "目标坐标：" + nPosition);
|
ret = GTSCardAPI.GT_SetPos((short)IConfig.CardNum, (short)axisNum, nPosition * IConfig.AxisVelocityRatio);
|
|
ret += GTSCardAPI.GT_Update((short)IConfig.CardNum, 1 << (axisNum - 1));
|
if (ret != (Int32)APS_Define.ResultSuccess)
|
{
|
LogAsync(DateTime.Now, "轴" + axisNum + "APS_absolute_move异常", "返回值：" + ret + "；重试次数：" + repeatTime);
|
Thread.Sleep(50);
|
}
|
repeatTime--;
|
} while (ret != (Int32)APS_Define.ResultSuccess && repeatTime > 0);
|
|
if (ret != (Int32)APS_Define.ResultSuccess)
|
{
|
LogAsync(DateTime.Now, "轴" + axisNum + "启动绝对运动异常", "错误码：" + ret);
|
throw new Exception("轴" + axisNum + "启动绝对运动异常，错误码：" + ret);
|
}
|
}
|
}
|
catch (Exception ex)
|
{
|
MoveStop();
|
OnExceptionRaised?.Invoke(ex);
|
}
|
}
|
|
Dictionary<int, AxisMovingStay> _axisStayDict = new Dictionary<int, AxisMovingStay>();
|
|
private async void MoveAbsStay(int axisNum)
|
{
|
await Task.Run(() =>
|
{
|
while (CurrentState != EnumHelper.DeviceState.DSClose)
|
{
|
if (!_axisStayDict.ContainsKey(axisNum))
|
{
|
_axisStayDict[axisNum] = new AxisMovingStay();
|
}
|
|
_axisStayDict[axisNum].MoveHandle.WaitOne();
|
MoveAbsAsync(axisNum, _axisStayDict[axisNum].Position);
|
_axisStayDict[axisNum].MoveSendHandle.Set();
|
}
|
});
|
}
|
|
/// <summary>
|
/// 动作结束
|
/// </summary>
|
/// <param name="axisNum"></param>
|
/// <returns></returns>
|
public void RunFinish(int axisNum, bool isAbs = true)
|
{
|
MOTION_IO_STATUS MotionIoSts = new MOTION_IO_STATUS();
|
MOTION_BOOL_STATUS MotionSts = new MOTION_BOOL_STATUS();
|
var axisSetting = IConfig.AxisSettings.FirstOrDefault(a => a.AxisIndex == axisNum);
|
int startTime = System.Environment.TickCount;
|
|
//MDN信号
|
if (axisSetting.IsUseMDNStopSignal)
|
{
|
while (GetMotionIoStatus(axisNum, ref MotionIoSts) && GetMotionStatus(axisNum, ref MotionSts))
|
{
|
if (MotionIoSts.EMG)
|
{
|
LogAsync(DateTime.Now, "轴" + axisNum + "急停", "");
|
throw new Exception("轴" + axisNum + "急停");
|
}
|
|
if (MotionSts.MDN)
|
{
|
int stopCode = -1;
|
APS168.APS_get_stop_code(axisNum, ref stopCode);
|
|
LogAsync(DateTime.Now, "轴" + axisNum + "StopCode：", stopCode.ToString());
|
|
//0 正常停止 4 正极限 5 负极限
|
if (new List<int>() { 0, 4, 5 }.Contains(stopCode))
|
{
|
if (new List<int>() { 4, 5 }.Contains(stopCode) && axisSetting.IsUseWarning)
|
{
|
MoveStop(true);
|
OnExceptionRaised?.Invoke(new Exception("轴" + axisNum + "运动至极限位置，stopCode：" + stopCode.ToString(), null));
|
return;
|
}
|
if (IConfig.ActionAfterDelay > 0)
|
{
|
Thread.Sleep(IConfig.ActionAfterDelay);
|
}
|
|
int feedBack = 0;
|
GetPosition(axisNum, ref feedBack);
|
LogAsync(DateTime.Now, "轴" + axisNum + "运动结束", "当前位置：" + feedBack);
|
break;
|
}
|
}
|
|
}
|
}
|
else //INP信号
|
{
|
while (GetMotionIoStatus(axisNum, ref MotionIoSts) && GetMotionStatus(axisNum, ref MotionSts))
|
{
|
if (MotionIoSts.EMG)
|
{
|
LogAsync(DateTime.Now, "轴" + axisNum + "急停", "");
|
throw new Exception("轴" + axisNum + "急停");
|
}
|
|
|
if (MotionSts.MDN && MotionIoSts.INP)
|
{
|
int stopCode = -1;
|
APS168.APS_get_stop_code(axisNum, ref stopCode);
|
|
LogAsync(DateTime.Now, "轴" + axisNum + "StopCode：", stopCode.ToString());
|
|
//0 正常停止 4 正极限 5 负极限
|
if (new List<int>() { 0, 4, 5 }.Contains(stopCode))
|
{
|
if (new List<int>() { 4, 5 }.Contains(stopCode) && axisSetting.IsUseWarning)
|
{
|
MoveStop(true);
|
OnExceptionRaised?.Invoke(new AMPRunException("轴" + axisNum + "运动至极限位置，stopCode：" + stopCode.ToString(), null));
|
return;
|
}
|
|
if (IConfig.ActionAfterDelay > 0)
|
{
|
Thread.Sleep(IConfig.ActionAfterDelay);
|
}
|
|
int feedBack = 0;
|
GetPosition(axisNum, ref feedBack);
|
LogAsync(DateTime.Now, "轴" + axisNum + "运动结束", "当前位置：" + feedBack);
|
break;
|
}
|
}
|
|
}
|
}
|
}
|
|
/// <summary>
|
/// Stop single axis move
|
/// </summary>
|
/// <param name="axisNum">axisNo</param>
|
/// <param name="option">0表示平滑停止，1表示紧急停止</param>
|
/// <returns></returns>
|
public void MoveStop(int axisNum, int option)
|
{
|
int ret = GTSCardAPI.GT_Stop((short)IConfig.CardNum, 1 << (axisNum - 1), option);
|
if (ret != (Int32)APS_Define.ResultSuccess)
|
{
|
LogAsync(DateTime.Now, "轴" + axisNum + "运动停止异常", "错误码：" + ret);
|
throw new Exception("轴" + axisNum + "运动停止异常，错误码：" + ret);
|
}
|
else
|
{
|
LogAsync(DateTime.Now, "轴" + axisNum + "运动停止", "");
|
}
|
}
|
|
static object motionIOLock = new object();
|
/// <summary>
|
/// Get single Axis Motion_Io status and motion status
|
/// </summary>
|
/// <param name="nAxis"></param>
|
/// <param name="nMotionSts"></param>
|
/// <returns></returns>
|
public bool GetMotionStatus(int nAxis, ref MOTION_BOOL_STATUS strcMotionSts)
|
{
|
lock (motionIOLock)
|
{
|
int nMotionSts = 0;
|
nMotionSts = APS168.APS_motion_status(nAxis);
|
if (nMotionSts < 0)
|
{
|
return false;
|
}
|
strcMotionSts.ASTP = Convert.ToBoolean(nMotionSts & (1 << (int)MOTION_STATUS.ASTP));
|
strcMotionSts.HMV = Convert.ToBoolean(nMotionSts & (1 << (int)MOTION_STATUS.HMV));
|
strcMotionSts.MDN = Convert.ToBoolean(nMotionSts & (1 << (int)MOTION_STATUS.MDN));
|
strcMotionSts.DIR = Convert.ToBoolean(nMotionSts & (1 << (int)MOTION_STATUS.DIR));
|
}
|
return true;
|
}
|
|
/// <summary>
|
/// Get single Axis Motion_Io status and motion status
|
/// </summary>
|
/// <param name="nAxis"></param>
|
/// <param name="nMotionIoSts"></param>
|
/// <returns></returns>
|
public bool GetMotionIoStatus(int nAxis, ref MOTION_IO_STATUS strcMotionIoSts)
|
{
|
lock (motionIOLock)
|
{
|
//Thread.Sleep(15);
|
int nMotionIoSts = 0;
|
nMotionIoSts = APS168.APS_motion_io_status(nAxis);
|
if (nMotionIoSts < 0)
|
{
|
return false;
|
}
|
|
bool isAlarm = Convert.ToBoolean(nMotionIoSts & (1 << (int)MOTION_IOSTATUS.ALM));
|
if (strcMotionIoSts.ALM != isAlarm)
|
{
|
strcMotionIoSts.ALM = isAlarm;
|
OnMotionAlarm?.Invoke(nAxis, isAlarm);
|
}
|
|
strcMotionIoSts.PEL = Convert.ToBoolean(nMotionIoSts & (1 << (int)MOTION_IOSTATUS.PEL));
|
strcMotionIoSts.MEL = Convert.ToBoolean(nMotionIoSts & (1 << (int)MOTION_IOSTATUS.MEL));
|
strcMotionIoSts.ORG = Convert.ToBoolean(nMotionIoSts & (1 << (int)MOTION_IOSTATUS.ORG));
|
strcMotionIoSts.EMG = Convert.ToBoolean(nMotionIoSts & (1 << (int)MOTION_IOSTATUS.EMG));
|
strcMotionIoSts.INP = Convert.ToBoolean(nMotionIoSts & (1 << (int)MOTION_IOSTATUS.INP));
|
strcMotionIoSts.SVON = Convert.ToBoolean(nMotionIoSts & (1 << (int)MOTION_IOSTATUS.SVON));
|
}
|
return true;
|
}
|
|
public void MoveStop(bool emergencyStop = false)
|
{
|
int option = 0;
|
if (emergencyStop)
|
{
|
option = 1;
|
StateChange(EnumHelper.DeviceState.DSExcept);
|
}
|
|
IConfig.AxisSettings.Where(a => a.IsAxisEnabled).ToList().ForEach(axisNum =>
|
{
|
MoveStop(axisNum.AxisIndex, option);
|
});
|
}
|
|
/// <summary>
|
/// 回原点
|
/// </summary>
|
/// <param name="cardn">卡号</param>
|
/// <param name="axisn">轴号</param>
|
/// <param name="homests">轴回原点状态</param>
|
public bool GoHome(short cardn, short axisn, short home_mode, short home_dir, int homeoffset)
|
{
|
try
|
{
|
GTSCardAPI.GT_ZeroPos(cardn, axisn, 1);
|
GTSCardAPI.THomePrm thomeprm;
|
GTSCardAPI.THomeStatus homests;
|
short rtn = GTSCardAPI.GT_GetHomePrm(cardn, axisn, out thomeprm);
|
thomeprm.mode = home_mode;//回零方式
|
thomeprm.moveDir = home_dir;//回零方向
|
thomeprm.edge = 0;
|
thomeprm.velHigh = 50;
|
thomeprm.velLow = 50;
|
thomeprm.acc = 50;
|
thomeprm.dec = 50;
|
thomeprm.searchHomeDistance = 9999999;//搜搜距离
|
thomeprm.homeOffset = 0; //偏移距离
|
thomeprm.escapeStep = 1000;
|
rtn = GTSCardAPI.GT_GoHome(cardn, axisn, ref thomeprm); //启动回零
|
|
while (true)
|
{
|
Thread.Sleep(5);
|
GTSCardAPI.GT_GetHomeStatus(cardn, axisn, out homests);
|
if (homests.run == 0)
|
{
|
if (homests.error == 0)
|
{
|
Thread.Sleep(200);
|
GTSCardAPI.GT_ZeroPos(cardn, axisn, 1);
|
}
|
return true;
|
}
|
}
|
}
|
catch (Exception ex)
|
{
|
MoveStop();
|
OnExceptionRaised?.Invoke(ex);
|
return false;
|
}
|
}
|
|
/// <summary>
|
/// 回原点
|
/// </summary>
|
/// <param name="axisId"></param>
|
public void StartHoming(int axisId)
|
{
|
try
|
{
|
//servo on
|
APS168.APS_set_servo_on(axisId, 1);
|
Thread.Sleep(500); // Wait stable.
|
|
// 2. Start home move
|
APS168.APS_home_move(axisId);
|
|
WaitHomeFinish(axisId);
|
|
axisDestination[axisId] = 0;
|
}
|
catch (Exception ex)
|
{
|
MoveStop();
|
OnExceptionRaised?.Invoke(ex);
|
}
|
}
|
|
/// <summary>
|
/// 回原点
|
/// </summary>
|
/// <param name="axisId"></param>
|
/// <param name="homeMode"></param>
|
/// <param name="homeDir"></param>
|
/// <param name="praCurve"></param>
|
/// <param name="praAcc"></param>
|
/// <param name="praVm"></param>
|
public void StartHoming(int axisId, int homeMode, int homeDir, double praCurve, double praAcc, double praVm)
|
{
|
// 1. Select home mode and config home parameters
|
APS168.APS_set_axis_param(axisId, (Int32)APS_Define.PRA_HOME_MODE, homeMode); // Set home mode
|
APS168.APS_set_axis_param(axisId, (Int32)APS_Define.PRA_HOME_DIR, homeDir); // Set home direction
|
APS168.APS_set_axis_param_f(axisId, (Int32)APS_Define.PRA_HOME_CURVE, praCurve); // Set acceleration paten (T-curve)
|
APS168.APS_set_axis_param_f(axisId, (Int32)APS_Define.PRA_HOME_ACC, praAcc); // Set homing acceleration rate
|
APS168.APS_set_axis_param_f(axisId, (Int32)APS_Define.PRA_HOME_VM, praVm); // Set homing maximum velocity.
|
APS168.APS_set_axis_param_f(axisId, (Int32)APS_Define.PRA_HOME_VO, praVm / 5); // Set homing VO speed
|
APS168.APS_set_axis_param_f(axisId, (Int32)APS_Define.PRA_HOME_EZA, 0); // Set EZ signal alignment (yes or no)
|
APS168.APS_set_axis_param_f(axisId, (Int32)APS_Define.PRA_HOME_SHIFT, 0); // Set home position shfit distance.
|
APS168.APS_set_axis_param_f(axisId, (Int32)APS_Define.PRA_HOME_POS, 0); // Set final home position.
|
|
StartHoming(axisId);
|
}
|
|
/// <summary>
|
/// 回原点结束
|
/// </summary>
|
/// <param name="axisNum"></param>
|
/// <returns></returns>
|
public void WaitHomeFinish(int axisNum)
|
{
|
MOTION_IO_STATUS MotionIoSts = new MOTION_IO_STATUS();
|
MOTION_BOOL_STATUS MotionSts = new MOTION_BOOL_STATUS();
|
var axisSetting = IConfig.AxisSettings.FirstOrDefault(a => a.AxisIndex == axisNum);
|
int startTime = System.Environment.TickCount;
|
while ((GetMotionIoStatus(axisNum, ref MotionIoSts)) && (GetMotionStatus(axisNum, ref MotionSts)))
|
{
|
if (MotionIoSts.EMG)
|
{
|
LogAsync(DateTime.Now, "轴复位中" + axisNum + "急停", "");
|
throw new Exception("轴复位中" + axisNum + "急停");
|
}
|
|
if (axisSetting.HomeMode == 0)
|
{
|
if (!MotionSts.HMV && MotionIoSts.ORG)
|
break;
|
}
|
else
|
{
|
if (!MotionSts.HMV)
|
break;
|
}
|
|
if (axisSetting.TimeOutHome < System.Environment.TickCount - startTime)
|
{
|
LogAsync(DateTime.Now, "轴复位中" + axisNum + "回原点超时", "");
|
MoveStop(axisNum);
|
throw new Exception("轴复位中" + axisNum + "回原点超时");
|
}
|
}
|
|
//ClearPosition(axisNum);
|
}
|
|
static object _ioLock = new object();
|
/// <summary>
|
/// 设置IO卡输出
|
/// </summary>
|
/// <param name="DINo">IO端口序号</param>
|
/// <param name="Value">设置值(true:高电平， false:低电平)</param>
|
/// <returns></returns>
|
public bool SetOutput(int DINo, bool Value)
|
{
|
if (OnCheckOutputAllowed?.Invoke(DINo, Value, new Dictionary<int, int>(axisDestination.ToDictionary(u => u.Key, u => u.Value))) ?? false)
|
{
|
OnExceptionRaised?.Invoke(new AMPRunException(DINo + "输出" + Value.ToString() + "不被允许", null));
|
return false;
|
}
|
|
int ret = -1;
|
lock (_ioLock)
|
{
|
ret = APS168.APS_write_d_channel_output(0, 0, DINo, Value ? 1 : 0);
|
}
|
if (ret < 0)
|
{
|
return false;
|
}
|
return true;
|
}
|
|
/// <summary>
|
/// 获取输入信号
|
/// </summary>
|
/// <param name="stateType"></param>
|
/// <returns></returns>
|
public bool[] GetInputState()
|
{
|
int diVaule = 0;
|
|
lock (_ioLock)
|
{
|
APS168.APS_read_d_input(0, 0, ref diVaule);
|
}
|
return GetBoolSig(diVaule);
|
//return new bool[16];
|
}
|
|
/// <summary>
|
/// 获取输出信号
|
/// </summary>
|
/// <param name="stateType"></param>
|
/// <returns></returns>
|
public bool[] GetOutputState()
|
{
|
int doVaule = 0;
|
|
lock (_ioLock)
|
{
|
APS168.APS_read_d_output(0, 0, ref doVaule);
|
}
|
|
return GetBoolSig(doVaule);
|
|
}
|
|
/// <summary>
|
/// 将无符号整型转换为Bool型数组
|
/// </summary>
|
/// <param name="sigStr"></param>
|
/// <returns></returns>
|
private bool[] GetBoolSig(int sigStr)
|
{
|
bool[] state = new bool[32];
|
|
for (int i = 0; i < 32; i++)
|
{
|
state[i] = (sigStr & (1 << i)) != 0;
|
}
|
return state;
|
}
|
|
/// <summary>
|
/// 将bool数组转换成无符号整型
|
/// </summary>
|
/// <param name="sigArray"></param>
|
/// <returns></returns>
|
private int GetintSig(bool[] sigArray)
|
{
|
int state = -1;
|
|
for (int i = 31; i > -1; i--)
|
{
|
state = (state << 1) + (sigArray[i] ? 1 : 0);
|
}
|
|
return state;
|
}
|
|
/// <summary>
|
/// 读取输入
|
/// </summary>
|
/// <param name="cardNum">卡号</param>
|
/// <param name="index">输入口</param>
|
/// <returns>有输入返回true，无输入返回false</returns>
|
public bool GetDi(short cardNum, short index)
|
{
|
int value;
|
GTSCardAPI.GT_GetDi(cardNum, GTSCardAPI.MC_GPI, out value);
|
if ((value & 1 << index) == 0) return true;//有输入返回true
|
else return false; //无输入返回false
|
}
|
|
/// <summary>
|
/// 输出
|
/// </summary>
|
/// <param name="cardNum">卡号</param>
|
/// <param name="index">输出口,返回1-16</param>
|
/// <param name="value">false表示输出，true表示关闭</param>
|
public void WriteOut(short cardNum, short index, bool value)
|
{
|
short outNum = (short)(index % 100 + 1);
|
if (value)
|
{
|
GTSCardAPI.GT_SetDoBit(cardNum, GTSCardAPI.MC_GPO, outNum, 0);
|
}
|
else
|
{
|
GTSCardAPI.GT_SetDoBit(cardNum, GTSCardAPI.MC_GPO, outNum, 1);
|
}
|
}
|
|
/// <summary>
|
/// 停止 某个轴
|
/// </summary>
|
/// <param name="cardNum">卡号</param>
|
/// <param name="axisNum">轴号</param>
|
/// <param name="option">0表示平滑停止，1表示紧急停止</param>
|
public void Stop(short cardNum, short axisNum, short option)
|
{
|
GTSCardAPI.GT_Stop(cardNum, 1 << (axisNum - 1), option);
|
}
|
|
/// <summary>
|
/// 停止 某个轴（异步）
|
/// </summary>
|
/// <param name="cardNum">卡号</param>
|
/// <param name="axisNum">轴号</param>
|
/// <param name="option">0表示平滑停止，1表示紧急停止</param>
|
public async Task StopAsync(short cardNum, short axisNum, short option)
|
{
|
await Task.Run(() =>
|
{
|
GTSCardAPI.GT_Stop(cardNum, 1 << (axisNum - 1), option);
|
});
|
}
|
|
/// <summary>
|
/// 停止 某个轴
|
/// </summary>
|
/// <param name="cardNum"></param>
|
/// <param name="axisNum">轴号</param>
|
/// <param name="value">停止方式，false表示平滑停止，true表示紧急停止</param>
|
public void Stop(short cardNum, short axisNum, bool emergencyStop)
|
{
|
if (emergencyStop)
|
{
|
GTSCardAPI.GT_Stop(cardNum, 1 << (axisNum - 1), 1 << (axisNum - 1));
|
}
|
else
|
{
|
GTSCardAPI.GT_Stop(cardNum, 1 << (axisNum - 1), 0);
|
}
|
}
|
|
/// <summary>
|
/// 停止 全部轴
|
/// </summary>
|
/// <param name="cardNum"></param>
|
/// <param name="value">停止方式，false表示平滑停止，true表示紧急停止</param>
|
public void StopAll(short cardNum, bool value)
|
{
|
for (short i = 1; i <= GTSCardParameter.AxisCount; i++)
|
{
|
Stop(cardNum, i, value);
|
}
|
}
|
|
/// <summary>
|
/// IO输出
|
/// </summary>
|
/// <param name="cardNum">卡号</param>
|
/// <param name="mdl">模块号</param>
|
/// <param name="index">IO输出</param>
|
/// <param name="value">true表示输出，false表示无输出</param>
|
public void MC_WriteDigitalOutput(short cardNum, short mdl, short index, bool value)
|
{
|
if (value)
|
{
|
GTSCardAPI.GT_SetExtIoBit(cardNum, mdl, index, 0);
|
}
|
else
|
{
|
GTSCardAPI.GT_SetExtIoBit(cardNum, mdl, index, 1);
|
}
|
}
|
|
/// <summary>
|
/// 读取IO输出状态
|
/// </summary>
|
/// <param name="cardNum"></param>
|
/// <param name="index"></param>
|
/// <returns></returns>
|
public bool GetDoSts(short cardNum, short index)
|
{
|
short outNum = 0;
|
int outSts;
|
outNum = (short)(index % 100);
|
GTSCardAPI.GT_GetDo(cardNum, GTSCardAPI.MC_GPO, out outSts);
|
if ((outSts & (1 << outNum)) == 0) return true;
|
else return false;
|
}
|
|
|
|
|
/// <summary>
|
/// 读取当前值
|
/// </summary>
|
/// <param name="cardNum">卡号</param>
|
/// <param name="axisNum">轴号</param>
|
/// <returns>返回当前值，单位毫米</returns>
|
public double GetPosMM(short cardNum, short axisNum)
|
{
|
lock (lockObj)
|
{
|
double prfpos = 0; uint pclock = 0;
|
GTSCardAPI.GT_GetPrfPos(cardNum, axisNum, out prfpos, 1, out pclock);
|
return prfpos / IConfig.AxisVelocityRatio;
|
}
|
}
|
|
/// <summary>
|
/// 读取轴状态，判断电机是否停止
|
/// </summary>
|
/// <param name="cardNum"></param>
|
/// <param name="axisNum"></param>
|
/// <returns></returns>
|
public bool IsStop(short cardNum, short axisNum)
|
{
|
lock (lockObj)
|
{
|
int sts = 0;
|
uint pclock = 0;
|
GTSCardAPI.GT_GetSts(cardNum, axisNum, out sts, 1, out pclock);
|
if ((sts & 0x400) == 0) return true;//停止返回true
|
else return false; //运行中返回false
|
}
|
}
|
#endregion
|
|
public void Monitor()
|
{
|
throw new NotImplementedException();
|
}
|
|
public void ResetAlarm()
|
{
|
throw new NotImplementedException();
|
}
|
}
|
}
|
