From 9e1247ca8890d904b0ebf0ee1565839e32166ec3 Mon Sep 17 00:00:00 2001
From: wells.liu <wells.liu@broconcentric.com>
Date: 星期二, 30 六月 2020 11:15:47 +0800
Subject: [PATCH] 固高驱动
---
src/Bro.Device.GTSCard/GTSCardDriver.cs | 237 +++++++++++++++++++++++++++++++++++++----------
src/Bro.Device.GTSCard/GTSCardConfig.cs | 6
2 files changed, 188 insertions(+), 55 deletions(-)
diff --git a/src/Bro.Device.GTSCard/GTSCardConfig.cs b/src/Bro.Device.GTSCard/GTSCardConfig.cs
index e56fd79..b440e1b 100644
--- a/src/Bro.Device.GTSCard/GTSCardConfig.cs
+++ b/src/Bro.Device.GTSCard/GTSCardConfig.cs
@@ -211,12 +211,12 @@
[Category("閫熷害閰嶇疆")]
[DisplayName("鍔犻�熷害")]
[Description("Acc锛氬姞閫熷害,涓�0鏃惰〃绀轰笉淇敼褰撳墠璁剧疆")]
- public double Acc { get; set; } = 0;
+ public double Acc { get; set; } = 1;
[Category("閫熷害閰嶇疆")]
[DisplayName("鍑忛�熷害")]
- [Description("鍑忛�熷害,涓�0鏃惰〃绀轰笉淇敼褰撳墠璁剧疆銆傚洖鍘熺偣妯″紡鏃惰缃负鏇茬嚎鍙傛暟")]
- public double Dec { get; set; } = 0;
+ [Description("鍑忛�熷害,涓�0鏃惰〃绀轰笉淇敼褰撳墠璁剧疆")]
+ public double Dec { get; set; } = 1;
}
/// <summary>
diff --git a/src/Bro.Device.GTSCard/GTSCardDriver.cs b/src/Bro.Device.GTSCard/GTSCardDriver.cs
index d4eea2e..e96d00c 100644
--- a/src/Bro.Device.GTSCard/GTSCardDriver.cs
+++ b/src/Bro.Device.GTSCard/GTSCardDriver.cs
@@ -68,12 +68,13 @@
protected override void Start()
{
- throw new NotImplementedException();
+ AllAxisOn();
}
protected override void Stop()
{
- throw new NotImplementedException();
+ AllMoveStop();
+ AllAxisOff();
}
#endregion
@@ -93,6 +94,60 @@
{
throw new Exception("鏉垮崱杞藉叆閰嶇疆鏂囦欢寮傚父锛岄敊璇爜锛�" + res);
}
+ }
+
+ public bool AllAxisOn()
+ {
+ List<Task<bool>> taskList = new List<Task<bool>>(); ;
+ // 濡傛灉鏄涓酱鐨勮繍鍔� 绛夋瘡涓酱寮�鍚�
+ IConfig.AxisSettings.Where(a => a.IsAxisEnabled).ToList().ForEach(axisNum =>
+ {
+ var task = AxisOnAsync((short)IConfig.CardNum, (short)axisNum.AxisIndex);
+ taskList.Add(task);
+ });
+ Task.WaitAll(taskList.ToArray());
+ var resultOK = taskList.All(u => u.GetAwaiter().GetResult());
+ return resultOK;
+ }
+
+ public bool AllAxisOff()
+ {
+ List<Task<bool>> taskList = new List<Task<bool>>(); ;
+ // 濡傛灉鏄涓酱鐨勮繍鍔� 绛夋瘡涓酱鍏抽棴
+ IConfig.AxisSettings.Where(a => a.IsAxisEnabled).ToList().ForEach(axisNum =>
+ {
+ var task = AxisOffAsync((short)IConfig.CardNum, (short)axisNum.AxisIndex);
+ taskList.Add(task);
+ });
+ Task.WaitAll(taskList.ToArray());
+ var resultOK = taskList.All(u => u.GetAwaiter().GetResult());
+ return resultOK;
+ }
+
+ /// <summary>
+ /// 鍗曚釜杞村紑鍚�
+ /// </summary>
+ /// <returns></returns>
+ public async Task<bool> AxisOnAsync(short cardNum, short axisNum)
+ {
+ return await Task.Run(() =>
+ {
+ var ret = GTSCardAPI.GT_AxisOn(cardNum, axisNum);
+ return ret == (short)GTSRetCode.GRCRunOK;
+ });
+ }
+
+ /// <summary>
+ /// 鍗曚釜杞村叧闂�
+ /// </summary>
+ /// <returns></returns>
+ public async Task<bool> AxisOffAsync(short cardNum, short axisNum)
+ {
+ return await Task.Run(() =>
+ {
+ var ret = GTSCardAPI.GT_AxisOff(cardNum, axisNum);
+ return ret == (short)GTSRetCode.GRCRunOK;
+ });
}
/// <summary>
@@ -123,14 +178,14 @@
}
/// <summary>
- /// Set AxisParam
+ /// 鐐瑰埌鐐硅繍鍔ㄨ缃弬鏁�
/// </summary>
/// <param name="optionPara">杩愬姩鍙傛暟瀵硅薄</param>
/// <returns></returns>
private bool SetAxisParam(MovingOption optionPara)
{
List<short> resultCode = new List<short>() { 0 };
- GTSCardAPI.TTrapPrm trapprm;
+ GTSCardAPI.TTrapPrm trapprm = new GTSCardAPI.TTrapPrm();
resultCode.Add(GTSCardAPI.GT_PrfTrap((short)IConfig.CardNum, (short)optionPara.AxisIndex));
resultCode.Add(GTSCardAPI.GT_GetTrapPrm((short)IConfig.CardNum, (short)optionPara.AxisIndex, out trapprm));
trapprm.smoothTime = 1;
@@ -149,7 +204,7 @@
}
/// <summary>
- /// 鍗曚釜杞� 鐐逛綅鍒扮偣浣嶈繍鍔�
+ /// 鍗曚釜杞� 杩愬姩(鐐瑰埌鐐� jog 鍥為浂...锛�
/// </summary>
/// <param name="optionPara">杩愬姩鍙傛暟瀵硅薄</param>
public async Task<bool> SingleAxisMoving(MovingOption optionPara)
@@ -173,24 +228,26 @@
LogAsync(DateTime.Now, "澶嶄綅涓惎鍔ㄨ繍鍔ㄥ紓甯�", optionPara.AxisIndex + "鍚姩杩愬姩寮傚父");
return false;
}
- //璁剧疆 杩愬姩鍙傛暟
- var isSuccess = SetAxisParam(optionPara);
- if (isSuccess)
+
+ if (optionPara.IsAbsolute)
{
- if (optionPara.IsAbsolute)
- {
- isSuccessAndStop = MoveAbs(optionPara);
- }
- else
- {
- isSuccessAndStop = MoveRel(optionPara);
- }
+ isSuccessAndStop = P2PMoveAbs(optionPara);
}
+ else
+ {
+ isSuccessAndStop = P2PMoveRel(optionPara);
+ }
+
}
break;
case MotorMoveMode.FindOri:
{
isSuccessAndStop = GoHome(optionPara);
+ }
+ break;
+ case MotorMoveMode.Jog:
+ {
+ isSuccessAndStop = JogMove(optionPara);
}
break;
}
@@ -252,21 +309,38 @@
/// <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)
+ public bool JogMove(MovingOption optionPara)
{
- 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)GTSRetCode.GRCRunOK)
+ try
{
+ GTSCardAPI.TJogPrm jogprm = new GTSCardAPI.TJogPrm();
+ short ret = 0;
+ int repeatTime = 100;
+ do
+ {
+ ret = GTSCardAPI.GT_PrfJog((short)IConfig.CardNum, (short)optionPara.AxisIndex);
+ jogprm.acc = optionPara.VelocityPara.Acc;
+ jogprm.dec = optionPara.VelocityPara.Dec;
+ ret = GTSCardAPI.GT_SetJogPrm((short)IConfig.CardNum, (short)optionPara.AxisIndex, ref jogprm);//璁剧疆jog杩愬姩鍙傛暟
+ ret = GTSCardAPI.GT_SetVel((short)IConfig.CardNum, (short)optionPara.AxisIndex, optionPara.VelocityPara.Velocity);//璁剧疆鐩爣閫熷害
+ ret = GTSCardAPI.GT_Update((short)IConfig.CardNum, 1 << (optionPara.AxisIndex - 1));//鏇存柊杞磋繍鍔�
+
+ if (ret != (short)GTSRetCode.GRCRunOK)
+ {
+ LogAsync(DateTime.Now, "杞�" + optionPara.AxisIndex + "APS_absolute_move寮傚父", "閿欒鐮侊細" + ret + ";" + "閲嶈瘯娆℃暟锛�" + repeatTime);
+ Thread.Sleep(50);
+ }
+ repeatTime--;
+ } while (ret != (short)GTSRetCode.GRCRunOK && repeatTime > 0);
+ return (ret == (short)GTSRetCode.GRCRunOK);
+ }
+ catch (Exception ex)
+ {
+ AllMoveStop(true);
+ OnExceptionRaised?.Invoke(ex);
return false;
}
- return true;
+
}
/// <summary>
@@ -297,7 +371,7 @@
/// <param name="axisNum">AxisNo</param>
/// <param name="nDistance">run distance</param>
/// <returns></returns>
- public bool MoveRel(MovingOption optionPara)
+ public bool P2PMoveRel(MovingOption optionPara)
{
try
{
@@ -328,11 +402,14 @@
LogAsync(DateTime.Now, "杞�" + optionPara.AxisIndex + "寮�濮嬭繍鍔�", "鐩爣鍧愭爣锛�" + optionPara.Destination);
short ret = 0;
- repeatTime = 1000;
+ repeatTime = 100;
+ bool isSuccessSetAxisParam = false;
int currentPosition = (int)GetPosition(optionPara.AxisIndex);
int dPosition = optionPara.Destination + currentPosition;
do
{
+ //璁剧疆 杩愬姩鍙傛暟
+ isSuccessSetAxisParam = SetAxisParam(optionPara);
ret = GTSCardAPI.GT_SetPrfPos((short)IConfig.CardNum, (short)optionPara.AxisIndex, (int)(dPosition * IConfig.AxisVelocityRatio));// 璁剧疆瑙勫垝浣嶇疆
ret = GTSCardAPI.GT_Update((short)IConfig.CardNum, 1 << (optionPara.AxisIndex - 1));//鏇存柊杩愬姩
@@ -342,7 +419,7 @@
Thread.Sleep(50);
}
repeatTime--;
- } while (ret != (short)GTSRetCode.GRCRunOK && repeatTime > 0);
+ } while (ret != (short)GTSRetCode.GRCRunOK && !isSuccessSetAxisParam && repeatTime > 0);
//杩愬姩寮�濮嬪悗 妫�鏌ヨ繍鍔ㄦ槸鍚﹀仠姝�
bool isStop = false;
@@ -368,7 +445,7 @@
/// 缁濆浣嶇疆杩愬姩
/// </summary>
/// <param name="optionPara">杩愬姩鍙傛暟瀵硅薄</param>
- public bool MoveAbs(MovingOption optionPara)
+ public bool P2PMoveAbs(MovingOption optionPara)
{
try
{
@@ -396,9 +473,12 @@
}
LogAsync(DateTime.Now, "杞�" + optionPara.AxisIndex + "寮�濮嬭繍鍔�", "鐩爣鍧愭爣锛�" + optionPara.Destination);
short ret = 0;
- repeatTime = 1000;
+ repeatTime = 100;
+ bool isSuccessSetAxisParam = false;
do
{
+ //璁剧疆 杩愬姩鍙傛暟
+ isSuccessSetAxisParam = SetAxisParam(optionPara);
ret = GTSCardAPI.GT_SetPrfPos((short)IConfig.CardNum, (short)optionPara.AxisIndex, (int)(optionPara.Destination * IConfig.AxisVelocityRatio));// 璁剧疆瑙勫垝浣嶇疆
ret = GTSCardAPI.GT_Update((short)IConfig.CardNum, 1 << (optionPara.AxisIndex - 1));//鏇存柊杩愬姩
@@ -408,7 +488,7 @@
Thread.Sleep(50);
}
repeatTime--;
- } while (ret != (short)GTSRetCode.GRCRunOK && repeatTime > 0);
+ } while (ret != (short)GTSRetCode.GRCRunOK && !isSuccessSetAxisParam && repeatTime > 0);
bool isStop = false;
repeatTime = 1000;
@@ -435,36 +515,53 @@
/// <param name="axisNum">axisNo</param>
/// <param name="option">0琛ㄧず骞虫粦鍋滄锛�1琛ㄧず绱ф�ュ仠姝�</param>
/// <returns></returns>
- public void MoveStop(int axisNum, int option)
+ public async Task<bool> MoveStop(int axisNum, int option)
{
- if (option == 1)
+ return await Task.Run(() =>
{
- StateChange(EnumHelper.DeviceState.DSExcept);
- }
- var ret = GTSCardAPI.GT_Stop((short)IConfig.CardNum, 1 << (axisNum - 1), option);
- if (ret != (short)GTSRetCode.GRCRunOK)
- {
- LogAsync(DateTime.Now, "杞�" + axisNum + "杩愬姩鍋滄寮傚父", "閿欒鐮侊細" + ret);
- throw new Exception("杞�" + axisNum + "杩愬姩鍋滄寮傚父锛岄敊璇爜锛�" + ret);
- }
- else
- {
- LogAsync(DateTime.Now, "杞�" + axisNum + "杩愬姩鍋滄", "");
- }
+ bool isStop = false;
+ if (option == 1)
+ {
+ StateChange(EnumHelper.DeviceState.DSExcept);
+ }
+ var ret = GTSCardAPI.GT_Stop((short)IConfig.CardNum, 1 << (axisNum - 1), option);
+ if (ret != (short)GTSRetCode.GRCRunOK)
+ {
+ LogAsync(DateTime.Now, "杞�" + axisNum + "杩愬姩鍋滄寮傚父", "閿欒鐮侊細" + ret);
+ throw new Exception("杞�" + axisNum + "杩愬姩鍋滄寮傚父锛岄敊璇爜锛�" + ret);
+ }
+ else
+ {
+ LogAsync(DateTime.Now, "杞�" + axisNum + "杩愬姩鍋滄", "");
+ }
+ int repeatTime = 100;
+ do
+ {
+ isStop = IsStop((short)IConfig.CardNum, (short)axisNum);
+ Thread.Sleep(10);
+ repeatTime--;
+ } while (!isStop && repeatTime > 0);
+
+ return (ret == (short)GTSRetCode.GRCRunOK) && isStop;
+ });
}
/// <summary>
- /// 鎵�鏈夊紑鍚殑杞村叧闂�
+ /// 鎵�鏈夊紑鍚殑杞村仠姝�
/// </summary>
/// <param name="emergencyStop"></param>
public void AllMoveStop(bool emergencyStop = false)
{
int option = emergencyStop ? 1 : 0;
-
+ List<Task<bool>> taskList = new List<Task<bool>>(); ;
+ // 濡傛灉鏄涓酱鐨勮繍鍔� 绛夋瘡涓酱杩愬姩缁撴潫
IConfig.AxisSettings.Where(a => a.IsAxisEnabled).ToList().ForEach(axisNum =>
{
- MoveStop(axisNum.AxisIndex, option);
+ var task = MoveStop(axisNum.AxisIndex, option);
+ taskList.Add(task);
});
+ Task.WaitAll(taskList.ToArray());
+ var resultOK = taskList.All(u => u.GetAwaiter().GetResult());
}
/// <summary>
@@ -632,9 +729,45 @@
public void ResetAlarm()
{
- throw new NotImplementedException();
- }
+ int axis_sts;
+ uint clk;
+ var axisSettings = IConfig.AxisSettings.FindAll(u => u.IsAxisEnabled);
+ GTSCardAPI.GT_ClrSts((short)IConfig.CardNum, 1, (short)axisSettings.Count);
+ foreach (var axisSetting in axisSettings)
+ {
+ GTSCardAPI.GT_GetSts((short)IConfig.CardNum, (short)axisSetting.AxisIndex, out axis_sts, 1, out clk);
+ if ((axis_sts & 0x200) == 0)
+ {
+ var rst = GTSCardAPI.GT_AxisOn((short)IConfig.CardNum, (short)axisSetting.AxisIndex);
+ }
+ // 浣嶇疆璇烽浂
+ GTSCardAPI.GT_ZeroPos((short)IConfig.CardNum, 1, (short)axisSettings.Count);
+ // 姝ｆ瀬闄愭姤璀�
+ if ((axis_sts & 0x20) != 0)
+ {
+ // 璐熷悜绉诲姩
+ MovingOption movingOption = new MovingOption();
+ movingOption.AxisIndex = (short)axisSetting.AxisIndex;
+ movingOption.Destination = -50; // 璐熷悜绉诲姩
+ movingOption.VelocityPara.Velocity = 50;
+ P2PMoveAbs(movingOption);
+ }
+ // 璐熸瀬闄愭姤璀�
+ if ((axis_sts & 0x40) != 0)
+ {
+ // 姝ｅ悜绉诲姩
+ MovingOption movingOption = new MovingOption();
+ movingOption.AxisIndex = (short)axisSetting.AxisIndex;
+ movingOption.Destination = 50; // 璐熷悜绉诲姩
+ movingOption.VelocityPara.Velocity = 50;
+ P2PMoveAbs(movingOption);
+ }
+ }
+
+ // 娓呴櫎鐘舵��
+ GTSCardAPI.GT_ClrSts((short)IConfig.CardNum, 1, (short)IConfig.AxisSettings.FindAll(u => u.IsAxisEnabled).Count);
+ }
}
}
--
Gitblit v1.8.0