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");
}
}
}