From 0d97500a3aac13b642fc93fae2e5dd01e1086b21 Mon Sep 17 00:00:00 2001 From: wells.liu <wells.liu@broconcentric.com> Date: 星期一, 29 六月 2020 12:31:12 +0800 Subject: [PATCH] 暂存 固高板卡 --- src/Bro.Device.GTSCard/GTSCardDriver.cs | 1037 ++++++++++++++++++++++++++++++++++++++++++++++++++++++--- 1 files changed, 973 insertions(+), 64 deletions(-) diff --git a/src/Bro.Device.GTSCard/GTSCardDriver.cs b/src/Bro.Device.GTSCard/GTSCardDriver.cs index a51429e..03b4c15 100644 --- a/src/Bro.Device.GTSCard/GTSCardDriver.cs +++ b/src/Bro.Device.GTSCard/GTSCardDriver.cs @@ -1,27 +1,101 @@ 锘縰sing 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 { - public class GTSCardDriver : DeviceBase,IMonitor, IMotion + [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() { - throw new NotImplementedException(); + 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() @@ -43,43 +117,874 @@ { throw new NotImplementedException(); } + #endregion - /// <summary> - /// 鐐逛綅鍒扮偣浣� 杩愬姩 - /// </summary> - /// <param name="cardNum">鍗″彿</param> - /// <param name="axisNum">杞村彿</param> - /// <param name="prfPosition">瑙勫垝浣嶇疆,鍗曚綅姣背</param> - /// <param name="prfVelocity">瑙勫垝閫熷害,鍗曚綅绫虫瘡绉�</param> - public void P2P(short cardNum, short axisNum, int prfPosition, int prfVelocity) + #region GTSCard + + public void ClearPosition(short cardNum, short axisNum) { - GTSCardAPI.TTrapPrm trapprm; - GTSCardAPI.GT_PrfTrap(cardNum, axisNum); - GTSCardAPI.GT_GetTrapPrm(cardNum, axisNum, out trapprm); - trapprm.acc = GTSCardParameter.P2PAcc; - trapprm.dec = GTSCardParameter.P2PDec; - trapprm.smoothTime = 1; - GTSCardAPI.GT_SetTrapPrm(cardNum, axisNum, ref trapprm); - GTSCardAPI.GT_SetPos(cardNum, axisNum, prfPosition * GTSCardParameter.Dangliang); - GTSCardAPI.GT_SetVel(cardNum, axisNum, prfVelocity * GTSCardParameter.Dangliang); - GTSCardAPI.GT_Update(cardNum, 1 << (axisNum - 1)); + int ret = GTSCardAPI.GT_SetPos(cardNum, axisNum, 0); } /// <summary> - /// Jog杩愬姩 + /// Load Motion Card parameter from file /// </summary> - /// <param name="cardNum"></param> - /// <param name="axisNum"></param> - /// <param name="velocity">瑙勫垝閫熷害锛屽崟浣嶇背姣忕</param> - public void Jog(short cardNum, short axisNum, double velocity) + /// <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(cardNum, axisNum); + short rtn = GTSCardAPI.GT_PrfJog((short)IConfig.CardNum, (short)axisNum); jogprm.acc = 1; jogprm.dec = 1; - GTSCardAPI.GT_SetJogPrm(cardNum, axisNum, ref jogprm);//璁剧疆jog杩愬姩鍙傛暟 - GTSCardAPI.GT_SetVel(cardNum, axisNum, velocity);//璁剧疆鐩爣閫熷害 - GTSCardAPI.GT_Update(cardNum, 1 << (axisNum - 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 + "璇曞浘鍦ㄩ潪姝e父鐘舵�佽繍鍔�"); + throw new Exception("杞�" + axisNum + "璇曞浘鍦ㄩ潪姝e父鐘舵�佽繍鍔�", 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 姝e父鍋滄 4 姝f瀬闄� 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 姝e父鍋滄 4 姝f瀬闄� 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> + /// 灏嗘棤绗﹀彿鏁村瀷杞崲涓築ool鍨嬫暟缁� + /// </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> + /// 灏哹ool鏁扮粍杞崲鎴愭棤绗﹀彿鏁村瀷 + /// </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> @@ -99,36 +1004,45 @@ /// <summary> /// 杈撳嚭 /// </summary> - /// <param name="cardNumo">鍗″彿</param> + /// <param name="cardNum">鍗″彿</param> /// <param name="index">杈撳嚭鍙�,杩斿洖1-16</param> - /// <param name="value">0琛ㄧず杈撳嚭锛�1琛ㄧず鍏抽棴</param> - public void WriteOut(short cardNumo, short index, bool value) + /// <param name="value">false琛ㄧず杈撳嚭锛宼rue琛ㄧず鍏抽棴</param> + public void WriteOut(short cardNum, short index, bool value) { short outNum = (short)(index % 100 + 1); - switch (value) + if (value) { - case true: - { - GTSCardAPI.GT_SetDoBit(cardNumo, GTSCardAPI.MC_GPO, outNum, 0);//鎸変綅杈撳嚭锛�0琛ㄧず杈撳嚭锛�1琛ㄧず鍏抽棴 - } - break; - case false: - { - GTSCardAPI.GT_SetDoBit(cardNumo, GTSCardAPI.MC_GPO, outNum, 1);//鎸変綅杈撳嚭锛�0琛ㄧず杈撳嚭锛�1琛ㄧず鍏抽棴 - } - break; + 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="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> @@ -137,21 +1051,15 @@ /// <param name="cardNum"></param> /// <param name="axisNum">杞村彿</param> /// <param name="value">鍋滄鏂瑰紡锛宖alse琛ㄧず骞虫粦鍋滄锛宼rue琛ㄧず绱ф�ュ仠姝�</param> - public void Stop(short cardNum, short axisNum, bool value) + public void Stop(short cardNum, short axisNum, bool emergencyStop) { - switch (value) + if (emergencyStop) { - case false: - { - GTSCardAPI.GT_Stop(cardNum, 1 << (axisNum - 1), 0); - } - break; - case true: - { - GTSCardAPI.GT_Stop(cardNum, 1 << (axisNum - 1), 1 << (axisNum - 1)); - - } - break; + GTSCardAPI.GT_Stop(cardNum, 1 << (axisNum - 1), 1 << (axisNum - 1)); + } + else + { + GTSCardAPI.GT_Stop(cardNum, 1 << (axisNum - 1), 0); } } @@ -190,21 +1098,21 @@ /// <summary> /// 璇诲彇IO杈撳嚭鐘舵�� /// </summary> - /// <param name="cardNumo"></param> + /// <param name="cardNum"></param> /// <param name="index"></param> /// <returns></returns> - public bool GetDoSts(short cardNumo, short index) + public bool GetDoSts(short cardNum, short index) { short outNum = 0; int outSts; outNum = (short)(index % 100); - GTSCardAPI.GT_GetDo(cardNumo, GTSCardAPI.MC_GPO, out outSts); + GTSCardAPI.GT_GetDo(cardNum, GTSCardAPI.MC_GPO, out outSts); if ((outSts & (1 << outNum)) == 0) return true; else return false; } - static object lockObj = new object(); - + + /// <summary> /// 璇诲彇褰撳墠鍊� @@ -218,7 +1126,7 @@ { double prfpos = 0; uint pclock = 0; GTSCardAPI.GT_GetPrfPos(cardNum, axisNum, out prfpos, 1, out pclock); - return prfpos / GTSCardParameter.Dangliang; + return prfpos / IConfig.AxisVelocityRatio; } } @@ -239,6 +1147,7 @@ else return false; //杩愯涓繑鍥瀎alse } } + #endregion public void Monitor() { -- Gitblit v1.8.0