using Aitex.Core.RT.Event;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.SCCore;
using MECF.Framework.Common.CommonData.PUF;
using MECF.Framework.Common.TwinCat;
using CyberX8_Core;
using System;
using System.Collections.Generic;
using System.Data;
using System.Linq;
using System.Reflection;
using System.Text;
using System.Threading.Tasks;
using CyberX8_RT.Devices.AXIS.Maxon;
namespace CyberX8_RT.Devices.AXIS.CANOpen
{
public class MaxonAxis : JetAxisBase
{
#region 内部变量
/// <summary>
/// Home routine
/// </summary>
private MaxonHomeRoutine _homeRoutine;
/// <summary>
/// 运动Routine
/// </summary>
private MaxonProfilePositionRoutine _profilePositionRoutine;
/// <summary>
/// Switch On Routine
/// </summary>
private MaxonSwitchOnRoutine _switchOnRoutine;
/// <summary>
/// Switch off Routine
/// </summary>
private MaxonSwitchOffRoutine _switchOffRoutine;
/// <summary>
/// Stop Position
/// </summary>
private MaxonStopPositionRoutine _stopPositionRoutine;
#endregion
/// <summary>
/// 构造函数
/// </summary>
/// <param name="Module"></param>
public MaxonAxis(string Module,string name):base(Module,name)
{
}
/// <summary>
/// 初始化参数
/// </summary>
protected override void InitializeParameter()
{
if(SC.ContainsItem("Maxon.SpeedRatio"))
{
_speedRatio = SC.GetValue<int>("Maxon.SpeedRatio");
}
if(SC.ContainsItem("Maxon.TorqueRatio"))
{
_torqueRatio = SC.GetValue<int>("Maxon.TorqueRatio");
}
}
/// <summary>
/// 初始化Routine
/// </summary>
protected override void InitializeRoutine()
{
_homeRoutine = new MaxonHomeRoutine($"{Module}.{Name}", this);
_profilePositionRoutine = new MaxonProfilePositionRoutine($"{Module}.{Name}", this);
_switchOnRoutine = new MaxonSwitchOnRoutine($"{Module}.{Name}", this);
_switchOffRoutine = new MaxonSwitchOffRoutine($"{Module}.{Name}", this);
_stopPositionRoutine = new MaxonStopPositionRoutine($"{Module}.{Name}", this);
}
/// <summary>
/// 中止操作
/// </summary>
/// <param name="cmd"></param>
/// <param name="args"></param>
/// <returns></returns>
public override bool StopPositionOperation()
{
if(!IsRun)
{
return true;
}
if (_profilePositionRoutine.Monitor() == RState.Running)
{
_profilePositionRoutine.Abort();
}
_status = _stopPositionRoutine.Start();
_currentOperation = MotionOperation.StopPosition;
return true;
}
/// <summary>
/// 更新StatusWord
/// </summary>
/// <param name="status"></param>
public override void UpdateStatusWord(ushort status)
{
if (status == 0)
{
_commandMotionData.Status = "Status Word is zero";
return;
}
byte bit0 = (byte)(status & 0b0001);
byte bit1 = (byte)((status & 0b0010) >> 1);
byte bit2 = (byte)((status & 0b0100) >> 2);
byte bit3 = (byte)((status & 0b1000) >> 3);
byte bit4 = (byte)((status & 0b10000) >> 4);
byte bit5 = (byte)((status & 0b100000) >> 5);
byte bit6 = (byte)((status & 0b1000000) >> 6);
byte bit7 = (byte)((status & 0b10000000) >> 7);
byte bit8 = (byte)((status & 0b100000000) >> 8);
byte bit9 = (byte)((status & 0b1000000000) >> 9);
byte bit10 = (byte)((status & 0b10000000000) >> 10);
byte bit11 = (byte)((status & 0b100000000000) >> 11);
byte bit12 = (byte)((status & 0b1000000000000) >> 12);
byte bit13 = (byte)((status & 0b10000000000000) >> 13);
byte bit14 = (byte)((status & 0b100000000000000) >> 14);
if (bit0 == 0 && bit1 == 0 && bit2 == 0 && bit3 == 0&&bit4==0&&bit5==0&&bit8==1&&bit14==0)
{
if (bit6 == 0)
{
_commandMotionData.Status = "Not Ready to switch On";
}
else
{
_commandMotionData.Status = "Switch On Disabled";
}
UpdateSwitchOn(false);
}
else if (bit0 == 1 && bit2 == 0 && bit3 == 0 && bit5 == 1 && bit6 == 0&&bit8==1&&bit14==0)
{
_commandMotionData.Status = "Ready to Switch On";
UpdateSwitchOn(false);
}
else if (bit0 == 1 && bit1 == 1 && bit2 == 0 && bit3 == 0 && bit5 == 1 && bit6 == 0 && bit8 == 1 && bit14 == 0)
{
_commandMotionData.Status = "Switched On";
UpdateSwitchOn(true);
}
else if (bit0 == 1 && bit1 == 1 && bit2 == 1 && bit3 == 0 &&bit4==1&& bit5 == 1 && bit6 == 0&&bit8==1&&bit14==0)
{
_commandMotionData.Status = "Operation Enabled";
UpdateSwitchOn(true);
}
else if (bit0 == 1 && bit1 == 1 && bit2 == 1 && bit3 == 0 &&bit4==1&& bit5 == 0 && bit6 == 0&&bit8==1&&bit14==0)
{
_commandMotionData.Status = "Quick Stop Active";
UpdateSwitchOn(true);
}
else if (bit0 == 1 && bit1 == 1 && bit2 == 1 && bit3 == 1 && bit6 == 0)
{
_commandMotionData.Status = "Fault reaction active";
UpdateSwitchOn(false);
}
else if (bit0 == 0 && bit1 == 0 && bit2 == 0 && bit3 == 1 && bit6 == 0)
{
_commandMotionData.Status = "Fault";
UpdateSwitchOn(false);
}
if (bit3 == 1)
{
_isError = true;
}
else
{
_isError = false;
}
if (bit7 == 1)
{
_commandMotionData.Status = "Waring is occured";
}
if (_modeOfOperation == (byte)AxisModeOfOperation.HomingMode)
{
if (bit10 == 1 && bit12 == 1)
{
_isHomed = true;
}
else
{
_isHomed = false;
}
}
else if (_modeOfOperation == (byte)AxisModeOfOperation.ProfilePositionMode)
{
if (bit10 == 1)
{
_inTargetPosition = true;
}
}
}
/// <summary>
/// 更新上电状态
/// </summary>
private void UpdateSwitchOn(bool isSwitchOn)
{
if (!_isSwitchOn && isSwitchOn)
{
_commandMotionData.IsSwitchOn = true;
ConfirmOperationState(MotionOperation.SwitchOn);
}
else if (_isSwitchOn && !isSwitchOn)
{
_commandMotionData.IsSwitchOn = false;
ConfirmOperationState(MotionOperation.SwitchOff);
}
_isSwitchOn = isSwitchOn;
}
/// <summary>
/// EnableOperation
/// </summary>
public override bool EnableOperation()
{
return WriteControlWord(0x0F);
}
/// <summary>
/// Home
/// </summary>
public override bool Home()
{
bool result = base.Home();
if(!result)
{
return false;
}
_homeRoutine.Start(_homeTimeout,0);
MotionData.IsHomed = false;
IsHomeSwitchedTriggered = false;
return true;
}
/// <summary>
/// 停止
/// </summary>
public override void Stop()
{
}
/// <summary>
/// SwitchOff
/// </summary>
public override bool SwitchOff()
{
if (_status == RState.Running)
{
EV.PostAlarmLog($"{Module}.{Name}", eEvent.ERR_AXIS, $"{Module}.{Name} current execute {_currentOperation},cannot switchoff");
return false;
}
_currentOperation = MotionOperation.SwitchOff;
_switchOffRoutine.Start();
_status = RState.Running;
return true;
}
/// <summary>
/// SwitchOn
/// </summary>
public override bool SwitchOn()
{
if (_status == RState.Running)
{
EV.PostAlarmLog($"{Module}.{Name}", eEvent.ERR_AXIS, $"{Module}.{Name} current execute {_currentOperation},cannot SwitchOn");
return false;
}
_currentOperation = MotionOperation.SwitchOn;
_switchOnRoutine.Start();
_status = RState.Running;
return true;
}
/// <summary>
/// OnTimer 定时器执行
/// </summary>
public override bool OnTimer()
{
if(_status == RState.Running)
{
if(_currentOperation == MotionOperation.Position)
{
RState state = _profilePositionRoutine.Monitor();
if (state == RState.End)
{
_inTargetPosition = false;
EndOperation();
_status = RState.End;
LOG.WriteLog(eEvent.INFO_AXIS, $"{Module}.{Name}", $"Position Complete,Current Position {MotionData.MotorPosition}");
}
else if (state == RState.Failed || state == RState.Timeout)
{
_inTargetPosition = false;
EndOperation();
_status = RState.Failed;
LOG.WriteLog(eEvent.ERR_AXIS, $"{Module}.{Name}", $"Profile Position error {_profilePositionRoutine.ErrorMsg}");
}
}
else if(_currentOperation == MotionOperation.Home)
{
RState state = _homeRoutine.Monitor();
if (state==RState.End)
{
MotionData.IsHomed = true;
IsHomed = true;
EndOperation();
_status= RState.End;
}
else if(state==RState.Failed||state==RState.Timeout)
{
EndOperation();
_status= RState.Failed;
LOG.WriteLog(eEvent.ERR_AXIS, $"{Module}.{Name}", "Home error");
}
}
else if (_currentOperation == MotionOperation.SwitchOn)
{
RState state = _switchOnRoutine.Monitor();
if (state == RState.End)
{
EndOperation();
_status= RState.End;
}
else if (state == RState.Failed || state == RState.Timeout)
{
EndOperation();
_status= RState.Failed;
LOG.WriteLog(eEvent.ERR_AXIS, $"{Module}.{Name}", "Switch On error");
}
}
else if (_currentOperation == MotionOperation.SwitchOff)
{
RState state = _switchOffRoutine.Monitor();
if (state == RState.End)
{
EndOperation();
_status= RState.End;
}
else if (state == RState.Failed || state == RState.Timeout)
{
EndOperation();
_status=RState.Failed;
LOG.WriteLog(eEvent.ERR_AXIS, $"{Module}.{Name}", "Switch Off error");
}
}
else if (_currentOperation == MotionOperation.StopPosition)
{
RState state = _stopPositionRoutine.Monitor();
if (state == RState.End)
{
EndOperation();
_status = RState.End;
}
else if (state == RState.Failed || state == RState.Timeout)
{
EndOperation();
_status = RState.Failed;
LOG.WriteLog(eEvent.ERR_AXIS, $"{Module}.{Name}", "StopPosition error");
}
}
}
JudgeRunMonitor();
return true;
}
/// <summary>
/// 位置
/// </summary>
/// <param name="targetPoint"></param>
public override bool ProfilePosition(int targetPoint, int profileVelocity, int profileAcceleration, int profileDeceleration,bool judgeTorqueLimit=true)
{
if (_status == RState.Running)
{
EV.PostAlarmLog($"{Module}.{Name}", eEvent.ERR_AXIS, $"{Module}.{Name} current execute {_currentOperation},cannot profile position");
return false;
}
if(profileAcceleration==0)
{
profileAcceleration = _profileAcceleration;
}
if (profileDeceleration==0)
{
profileDeceleration = _profileDeceleration;
}
_status = _profilePositionRoutine.Start(targetPoint,profileVelocity,profileAcceleration,profileDeceleration,judgeTorqueLimit);
_currentOperation = MotionOperation.Position;
_inTargetPosition = false;
return true;
}
/// <summary>
/// 改变速度
/// </summary>
/// <param name="speed"></param>
/// <returns></returns>
public override bool ChangeSpeed(int speed)
{
bool result = WriteControlWord(0x2F);
if (!result)
{
return false;
}
result = BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.ProfileVelocity", speed);
if (!result)
{
return false;
}
result = WriteControlWord(0x3F);
return result;
}
/// <summary>
/// 改变速度加速度
/// </summary>
/// <param name="speed"></param>
/// <returns></returns>
public override bool ChangeSpeedAcceleration(int speed,int acceleration,int deceleration)
{
bool result = WriteControlWord(0x2F);
if (!result)
{
return false;
}
result = BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.ProfileVelocity", speed);
BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.{PROFILE_ACCEL}", acceleration);
BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.{PROFILE_DECEL}", deceleration);
if (!result)
{
return false;
}
result = WriteControlWord(0x3F);
return result;
}
/// <summary>
/// KeyDown事件
/// </summary>
/// <param name="arg"></param>
/// <param name="value"></param>
protected override void AxisKeyDown(string arg, double value)
{
switch (arg)
{
case PROFILE_VELOCITY:
_profileVelocity = CalculateMultiplySpeedRatio(CalculateValueMultiplyScale(value));
_commandMotionData.FileProfileVelocity = value;
TwincatCoeManager.Instance.WriteVariableValue($"{Module}.{Name}",arg, _profileVelocity);
break;
case PROFILE_ACCEL:
_profileAcceleration = CalculateDivideAccelerationRatio(CalculateValueMultiplyScale(value));
_commandMotionData.FileAcceleration = value;
TwincatCoeManager.Instance.WriteVariableValue($"{Module}.{Name}",arg, _profileAcceleration);
break;
case PROFILE_DECEL:
_profileDeceleration = CalculateDivideAccelerationRatio(CalculateValueMultiplyScale(value));
_commandMotionData.FileDeceleration = value;
TwincatCoeManager.Instance.WriteVariableValue($"{Module}.{Name}",arg, _profileDeceleration);
break;
case HOMING_VELOCITY:
_profileHomingVelocity = CalculateMultiplySpeedRatio(CalculateValueMultiplyScale(value));
_commandMotionData.FileHomingVelocity = value;
TwincatCoeManager.Instance.WriteVariableValue($"{Module}.{Name}", arg, _profileHomingVelocity);
break;
case HOMING_VELOCITY_SLOW:
_profileHomingVelocitySlow = CalculateMultiplySpeedRatio(CalculateValueMultiplyScale(value));
_commandMotionData.FileHomingVelocitySlow = value;
TwincatCoeManager.Instance.WriteVariableValue($"{Module}.{Name}", arg, _profileHomingVelocitySlow);
break;
case HOMING_ACCEL:
_profileHomingAccel = CalculateDivideAccelerationRatio(CalculateValueMultiplyScale(value));
_commandMotionData.FileHomingAccel = value;
TwincatCoeManager.Instance.WriteVariableValue($"{Module}.{Name}", arg, _profileHomingAccel);
break;
}
}
}
}