|
@@ -1,139 +1,138 @@
|
|
|
-using Aitex.Core.RT.Log;
|
|
|
-using Aitex.Core.RT.Routine;
|
|
|
-using PunkHPX8_Core;
|
|
|
-using MECF.Framework.Common.Beckhoff.AxisProvider;
|
|
|
-using MECF.Framework.Common.CommonData.PUF;
|
|
|
-using MECF.Framework.Common.Routine;
|
|
|
-using System;
|
|
|
-
|
|
|
-namespace PunkHPX8_RT.Devices.AXIS.Kollmorgen
|
|
|
-{
|
|
|
- public class KollmorgenStopPositionRoutine : RoutineBase, IRoutine
|
|
|
- {
|
|
|
- private enum StopPositionStep
|
|
|
- {
|
|
|
- SetHaltControlWord,
|
|
|
- CheckStop,
|
|
|
- WriteTargePosition,
|
|
|
- ResetHaltControlWord,
|
|
|
- CheckRun,
|
|
|
- NoneModeOfOperation,
|
|
|
- EnableOperation,
|
|
|
- End
|
|
|
- }
|
|
|
- #region 常量
|
|
|
- private const string TARGET_POSITION = "TargetPosition";
|
|
|
- #endregion
|
|
|
-
|
|
|
- #region 内部变量
|
|
|
- private JetAxisBase _axis;
|
|
|
- private int _timeout = 5000;
|
|
|
- private int _targetPosition;
|
|
|
- /// <summary>
|
|
|
- /// SRD rotation Provider对象
|
|
|
- /// </summary>
|
|
|
- private BeckhoffProviderAxis _rotationProviderAxis;
|
|
|
- private BeckhoffCommonAxis _beckhoffCommonAxis;
|
|
|
- #endregion
|
|
|
-
|
|
|
- public KollmorgenStopPositionRoutine(string module, JetAxisBase axis, BeckhoffCommonAxis beckhoffCommonAxis) : base(module)
|
|
|
- {
|
|
|
- _axis = axis;
|
|
|
- _beckhoffCommonAxis = beckhoffCommonAxis;
|
|
|
- }
|
|
|
-
|
|
|
- public void Abort()
|
|
|
- {
|
|
|
- Runner.Stop("Manual Abort");
|
|
|
- }
|
|
|
-
|
|
|
- public RState Monitor()
|
|
|
- {
|
|
|
- Runner.Run(StopPositionStep.SetHaltControlWord, () => { return _beckhoffCommonAxis.WriteControlWord(0X8F); }, () => { return CheckControlWord(0X8F); }, 1000)
|
|
|
- .WaitWithStopCondition(StopPositionStep.CheckStop, CheckStop, CheckErrorOrWarning, _timeout)
|
|
|
- .Run(StopPositionStep.WriteTargePosition, () => { return WriteTargetPosition(); }, NullFun, 100)
|
|
|
- .Run(StopPositionStep.ResetHaltControlWord, () => { return _beckhoffCommonAxis.WriteControlWord(0x3F); }, () => { return CheckControlWord(0x3F); }, 1000)
|
|
|
- .WaitWithStopCondition(StopPositionStep.CheckRun, CheckRunEnd, CheckRunStop, 5000)
|
|
|
- .Run(StopPositionStep.NoneModeOfOperation, () => { return _beckhoffCommonAxis.WriteModeOfMode(AxisModeOfOperation.None); }, () => { return CheckModeOfOperation((byte)AxisModeOfOperation.None); }, 1000)
|
|
|
- .Run(StopPositionStep.EnableOperation, () => { return _beckhoffCommonAxis.WriteControlWord(0x0F); }, () => { return CheckControlWord(0x0F); }, 1000)
|
|
|
- .End(StopPositionStep.End, NullFun, 100);
|
|
|
- return Runner.Status;
|
|
|
- }
|
|
|
-
|
|
|
- public bool CheckModeOfOperation(byte modeOfOperation)
|
|
|
- {
|
|
|
- return _axis.ModeOfOperation == modeOfOperation;
|
|
|
- }
|
|
|
-
|
|
|
- public bool CheckControlWord(ushort controlWord)
|
|
|
- {
|
|
|
- return _axis.ControlWord == controlWord;
|
|
|
- }
|
|
|
- /// <summary>
|
|
|
- /// 设置目标位置为当前位置
|
|
|
- /// </summary>
|
|
|
- /// <param name="targetPosition"></param>
|
|
|
+using Aitex.Core.RT.Log;
|
|
|
+using Aitex.Core.RT.Routine;
|
|
|
+using PunkHPX8_Core;
|
|
|
+using MECF.Framework.Common.Beckhoff.AxisProvider;
|
|
|
+using MECF.Framework.Common.CommonData.PUF;
|
|
|
+using MECF.Framework.Common.Routine;
|
|
|
+using System;
|
|
|
+
|
|
|
+namespace PunkHPX8_RT.Devices.AXIS.Kollmorgen
|
|
|
+{
|
|
|
+ public class KollmorgenStopPositionRoutine : RoutineBase, IRoutine
|
|
|
+ {
|
|
|
+ private enum StopPositionStep
|
|
|
+ {
|
|
|
+ SetHaltControlWord,
|
|
|
+ CheckStop,
|
|
|
+ Delay,
|
|
|
+ WriteTargePosition,
|
|
|
+ NoneModeOfOperation,
|
|
|
+ EnableOperation,
|
|
|
+ End
|
|
|
+ }
|
|
|
+ #region 常量
|
|
|
+ private const string TARGET_POSITION = "TargetPosition";
|
|
|
+ #endregion
|
|
|
+
|
|
|
+ #region 内部变量
|
|
|
+ private JetAxisBase _axis;
|
|
|
+ private int _timeout = 5000;
|
|
|
+ private int _targetPosition;
|
|
|
+ /// <summary>
|
|
|
+ /// SRD rotation Provider对象
|
|
|
+ /// </summary>
|
|
|
+ private BeckhoffProviderAxis _rotationProviderAxis;
|
|
|
+ private BeckhoffCommonAxis _beckhoffCommonAxis;
|
|
|
+ #endregion
|
|
|
+
|
|
|
+ public KollmorgenStopPositionRoutine(string module, JetAxisBase axis, BeckhoffCommonAxis beckhoffCommonAxis) : base(module)
|
|
|
+ {
|
|
|
+ _axis = axis;
|
|
|
+ _beckhoffCommonAxis = beckhoffCommonAxis;
|
|
|
+ }
|
|
|
+
|
|
|
+ public void Abort()
|
|
|
+ {
|
|
|
+ Runner.Stop("Manual Abort");
|
|
|
+ }
|
|
|
+
|
|
|
+ public RState Monitor()
|
|
|
+ {
|
|
|
+ Runner.Run(StopPositionStep.SetHaltControlWord, () => { return _beckhoffCommonAxis.WriteControlWord(0X10F); }, () => { return CheckControlWord(0X10F); }, 1000)
|
|
|
+ .WaitWithStopCondition(StopPositionStep.CheckStop, CheckStop, CheckErrorOrWarning, _timeout)
|
|
|
+ .Delay(StopPositionStep.Delay,_delay_1s)
|
|
|
+ .Run(StopPositionStep.WriteTargePosition, () => { return WriteTargetPosition(); }, NullFun, 100)
|
|
|
+ .Run(StopPositionStep.NoneModeOfOperation, () => { return _beckhoffCommonAxis.WriteModeOfMode(AxisModeOfOperation.None); }, () => { return CheckModeOfOperation((byte)AxisModeOfOperation.None); }, 1000)
|
|
|
+ .Run(StopPositionStep.EnableOperation, () => { return _beckhoffCommonAxis.WriteControlWord(0x0F); }, () => { return CheckControlWord(0x0F); }, 1000)
|
|
|
+
|
|
|
+ .End(StopPositionStep.End, NullFun, 100);
|
|
|
+ return Runner.Status;
|
|
|
+ }
|
|
|
+
|
|
|
+ public bool CheckModeOfOperation(byte modeOfOperation)
|
|
|
+ {
|
|
|
+ return _axis.ModeOfOperation == modeOfOperation;
|
|
|
+ }
|
|
|
+
|
|
|
+ public bool CheckControlWord(ushort controlWord)
|
|
|
+ {
|
|
|
+ return _axis.ControlWord == controlWord;
|
|
|
+ }
|
|
|
+ /// <summary>
|
|
|
+ /// 设置目标位置为当前位置
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="targetPosition"></param>
|
|
|
private bool WriteTargetPosition()
|
|
|
- {
|
|
|
- double scale = _rotationProviderAxis.ScaleFactor;
|
|
|
+ {
|
|
|
+ double scale = _rotationProviderAxis.ScaleFactor;
|
|
|
_targetPosition = (int)Math.Ceiling(_axis.MotionData.MotorPosition * scale);
|
|
|
- return _beckhoffCommonAxis.WriteVariable(TARGET_POSITION, _targetPosition);
|
|
|
- }
|
|
|
- /// <summary>
|
|
|
- /// 检查是否运动完成
|
|
|
- /// </summary>
|
|
|
- /// <returns></returns>
|
|
|
- private bool CheckRunEnd()
|
|
|
- {
|
|
|
- bool result = _axis.InTargetPosition && !_axis.IsRun;
|
|
|
- return result;
|
|
|
- }
|
|
|
- /// <summary>
|
|
|
- /// 检查是否运动完成
|
|
|
- /// </summary>
|
|
|
- /// <returns></returns>
|
|
|
- private bool CheckRunStop()
|
|
|
- {
|
|
|
- //没有到达目标同时没有运动
|
|
|
- bool isStop = !_axis.IsRun && !_axis.JudgeCurrentPositionIsInTargetPosition(_targetPosition);
|
|
|
- if (isStop)
|
|
|
- {
|
|
|
- ErrorMsg = "position is stop";
|
|
|
- return true;
|
|
|
- }
|
|
|
- return false;
|
|
|
- }
|
|
|
- /// <summary>
|
|
|
- /// 检查Halt是否停下来
|
|
|
- /// </summary>
|
|
|
- /// <returns></returns>
|
|
|
- private bool CheckStop()
|
|
|
- {
|
|
|
- return !_axis.IsRun;
|
|
|
+ return _beckhoffCommonAxis.WriteVariable(TARGET_POSITION, _targetPosition);
|
|
|
+ }
|
|
|
+ /// <summary>
|
|
|
+ /// 检查是否运动完成
|
|
|
+ /// </summary>
|
|
|
+ /// <returns></returns>
|
|
|
+ private bool CheckRunEnd()
|
|
|
+ {
|
|
|
+ bool result = _axis.InTargetPosition && !_axis.IsRun;
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+ /// <summary>
|
|
|
+ /// 检查是否运动完成
|
|
|
+ /// </summary>
|
|
|
+ /// <returns></returns>
|
|
|
+ private bool CheckRunStop()
|
|
|
+ {
|
|
|
+ //没有到达目标同时没有运动
|
|
|
+ bool isStop = !_axis.IsRun && !_axis.JudgeCurrentPositionIsInTargetPosition(_targetPosition);
|
|
|
+ if (isStop)
|
|
|
+ {
|
|
|
+ ErrorMsg = "position is stop";
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ /// <summary>
|
|
|
+ /// 检查Halt是否停下来
|
|
|
+ /// </summary>
|
|
|
+ /// <returns></returns>
|
|
|
+ private bool CheckStop()
|
|
|
+ {
|
|
|
+ return !_axis.IsRun;
|
|
|
+ }
|
|
|
+
|
|
|
+ /// 检验是否出错或告警
|
|
|
+ /// </summary>
|
|
|
+ /// <returns></returns>
|
|
|
+ private bool CheckErrorOrWarning()
|
|
|
+ {
|
|
|
+ bool result = _axis.MotionData.Status.ToLower().Contains("error") || _axis.MotionData.Status.ToLower().Contains("warning");
|
|
|
+ if (result)
|
|
|
+ {
|
|
|
+ LOG.WriteLog(eEvent.ERR_AXIS, Module, $"axis stop occur error or warning");
|
|
|
+ }
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+ public RState Start(params object[] objs)
|
|
|
+ {
|
|
|
+ _rotationProviderAxis = BeckhoffAxisProviderManager.Instance.GetAxisProvider($"{Module}");
|
|
|
+ if (_rotationProviderAxis == null)
|
|
|
+ {
|
|
|
+ LOG.WriteLog(eEvent.ERR_AXIS, Module, $"{Module} Axist Provider is not exist");
|
|
|
+ return RState.Failed;
|
|
|
+ }
|
|
|
+ return Runner.Start(Module, "Stop Position");
|
|
|
}
|
|
|
+ }
|
|
|
+}
|
|
|
|
|
|
- /// 检验是否出错或告警
|
|
|
- /// </summary>
|
|
|
- /// <returns></returns>
|
|
|
- private bool CheckErrorOrWarning()
|
|
|
- {
|
|
|
- bool result = _axis.MotionData.Status.ToLower().Contains("error") || _axis.MotionData.Status.ToLower().Contains("warning");
|
|
|
- if (result)
|
|
|
- {
|
|
|
- LOG.WriteLog(eEvent.ERR_AXIS, Module, $"axis stop occur error or warning");
|
|
|
- }
|
|
|
- return result;
|
|
|
- }
|
|
|
- public RState Start(params object[] objs)
|
|
|
- {
|
|
|
- _rotationProviderAxis = BeckhoffAxisProviderManager.Instance.GetAxisProvider($"{Module}");
|
|
|
- if (_rotationProviderAxis == null)
|
|
|
- {
|
|
|
- LOG.WriteLog(eEvent.ERR_AXIS, Module, $"{Module} Axist Provider is not exist");
|
|
|
- return RState.Failed;
|
|
|
- }
|
|
|
- return Runner.Start(Module, "Stop Position");
|
|
|
- }
|
|
|
- }
|
|
|
-}
|
|
|
-
|