using Aitex.Core.RT.Log; using Aitex.Core.RT.Routine; using MECF.Framework.Common.CommonData.PUF; using MECF.Framework.Common.Routine; using System; using System.Collections.Generic; using System.Linq; using System.Text; using System.Threading.Tasks; using PunkHPX8_Core; using PunkHPX8_RT.Devices.AXIS; namespace CyberX12_RT.Devices.AXIS.Kollmorgen { public class KollmorgenHomeRoutine : RoutineBase, IRoutine { private enum HomeStep { WriteOffline, WriteOnline, WriteControlWord, HomeModeOfOperation, WriteHomeControlWord, HomeDelay, CheckHome, LastDelay, NoneModeOfOperation, EnableOperation, WriteTargetPosition, End } #region 常量 private const string TARGET_POSITION = "TargetPosition"; #endregion #region 内部变量 private JetAxisBase _axis; private BeckhoffCommonAxis _beckhoffCommonAxis; private int _timeout = 5000; private byte _homingMethod = 0; private bool _isLogError = true; #endregion public KollmorgenHomeRoutine(string module, JetAxisBase axis, BeckhoffCommonAxis beckhofCommonAxis) : base(module) { _axis = axis; _beckhoffCommonAxis = beckhofCommonAxis; } public void Abort() { Runner.Stop("Manual Abort"); } public RState Monitor() { Runner .Run(HomeStep.WriteControlWord, () => { return _beckhoffCommonAxis.WriteControlWord(0x0F); }, () => { return CheckControlWord(0x0F); }, 1000) .Run(HomeStep.HomeModeOfOperation, () => { return _beckhoffCommonAxis.WriteModeOfMode(AxisModeOfOperation.HomingMode); }, () => { return CheckModeOfOperation((byte)AxisModeOfOperation.HomingMode); }, 1000) .Run(HomeStep.WriteHomeControlWord, () => { return _beckhoffCommonAxis.WriteControlWord(0x1F);}, () => { return CheckControlWord(0x1F); }, 1000) .Delay(HomeStep.HomeDelay, 100) .WaitWithStopCondition(HomeStep.CheckHome, CheckHome, CheckErrorOrWarning, _timeout, _isLogError) .Delay(HomeStep.LastDelay, 1000) .Run(HomeStep.NoneModeOfOperation, () => { return _beckhoffCommonAxis.WriteModeOfMode(AxisModeOfOperation.None); }, () => { return CheckModeOfOperation((byte)AxisModeOfOperation.None); }, 1000) .Run(HomeStep.EnableOperation, () => { return _beckhoffCommonAxis.WriteControlWord(0x0F); }, () => { return CheckControlWord(0x0F); }, 1000) .Run(HomeStep.WriteTargetPosition,WriteTargetPosition,_delay_1ms) .End(HomeStep.End, NullFun, 100); return Runner.Status; } public bool CheckModeOfOperation(byte modeOfOperation) { return _axis.ModeOfOperation == modeOfOperation; } public bool CheckControlWord(ushort controlWord) { return _axis.ControlWord == controlWord; } /// /// 设置目标位置为当前位置 /// /// private bool WriteTargetPosition() { return _beckhoffCommonAxis.WriteVariable(TARGET_POSITION, 0); } private bool CheckHome() { LOG.WriteLog(eEvent.INFO_AXIS, Module, $"StatusWord is {_axis.MotionData.StatusWord}"); _axis.UpdateStatusWord(_axis.MotionData.StatusWord); return _axis.IsHomed; } /// 检验是否出错或告警 /// /// 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 home occur error or warning"); } return result; } public RState Start(params object[] objs) { _timeout = (int)objs[0]; _homingMethod = (byte)objs[1]; if (objs.Length >= 3) { _isLogError = (bool)objs[2]; } return Runner.Start(Module, "Home"); } } }