PunkHPX8/PunkHPX8_RT/Devices/AXIS/Yaskawa/YaskawaAxis.cs

using Aitex.Core.RT.DataCenter;
using Aitex.Core.RT.Event;
using Aitex.Core.RT.Fsm;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.Routine;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Util;
using MECF.Framework.Common.CommonData.PUF;
using MECF.Framework.Common.TwinCat;
using PunkHPX8_Core;
using PunkHPX8_RT.Modules;
using System;
using System.Collections.Generic;
using System.Data;
using System.Linq;
using System.Reflection;
using System.Text;
using System.Threading.Tasks;
using PunkHPX8_RT.Devices.AXIS.Maxon;
using MECF.Framework.Common.Device;
using MECF.Framework.Common.Beckhoff.IOAxis;
using static Mono.Security.X509.X520;

namespace PunkHPX8_RT.Devices.AXIS.Yaskawa
{
    public class YaskawaAxis : JetAxisBase
    {
        #region 内部变量
        /// <summary>
        /// Home routine
        /// </summary>
        private YaskawaHomeRoutine _homeRoutine;
        /// <summary>
        /// 运动Routine
        /// </summary>
        private YaskawaProfilePositionRoutine _profilePositionRoutine;
        /// <summary>
        /// Switch On Routine
        /// </summary>
        private YaskawaSwitchOnRoutine _switchOnRoutine;
        /// <summary>
        /// Switch Off Routine
        /// </summary>
        private YaskawaSwitchOffRoutine _switchOffRoutine;
        /// <summary>
        /// Stop Position
        /// </summary>
        private YaskawaStopPositionRoutine _stopPositionRoutine;
        /// <summary>
        /// Beckhoff共用对象
        /// </summary>
        private BeckhoffCommonAxis _beckhoffCommonAxis;
        #endregion
        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="Module"></param>
        public YaskawaAxis(string module,string name):base(module,name)
        {
        }
        /// <summary>
        /// 初始化参数
        /// </summary>
        protected override void InitializeParameter()
        {
            _accelerationRatio = 10000;
            _beckhoffCommonAxis = new BeckhoffCommonAxis(Module, Name, this);
        }
        /// <summary>
        /// 初始化Routine
        /// </summary>
        protected override void InitializeRoutine()
        {
            _homeRoutine = new YaskawaHomeRoutine($"{Module}.{Name}", this,_beckhoffCommonAxis);
            _profilePositionRoutine = new YaskawaProfilePositionRoutine($"{Module}.{Name}", this, _beckhoffCommonAxis);
            _switchOnRoutine = new YaskawaSwitchOnRoutine($"{Module}.{Name}", this, _beckhoffCommonAxis);
            _switchOffRoutine = new YaskawaSwitchOffRoutine($"{Module}.{Name}", this,_beckhoffCommonAxis);
            _stopPositionRoutine = new YaskawaStopPositionRoutine($"{Module}.{Name}", this, _beckhoffCommonAxis);
        }
        /// <summary>
        /// 中止操作
        /// </summary>
        /// <param name="cmd"></param>
        /// <param name="args"></param>
        /// <returns></returns>
        public override bool StopPositionOperation()
        {
            lock (_operationLocker)
            {
                if (_currentOperation == MotionOperation.Position)
                {
                    _profilePositionRoutine.Abort();
                    _status = _stopPositionRoutine.Start();
                    _currentOperation = MotionOperation.StopPosition;
                }
                else if (_currentOperation == MotionOperation.Home)
                {
                    _homeRoutine.Abort();
                    _status = _homeRoutine.Start();
                    _currentOperation = MotionOperation.StopPosition;
                }
                else
                {
                    EndOperation();
                }                
            }
            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)
            {
                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)
            {
                _commandMotionData.Status = "Ready to Switch On";
                UpdateSwitchOn(false);
            }
            else if (bit0 == 1 && bit1 == 1 && bit2 == 0 && bit3 == 0 && bit5 == 1 && bit6 == 0)
            {
                _commandMotionData.Status = "Switched On";
                UpdateSwitchOn(true);
            }
            else if (bit0 == 1 && bit1 == 1 && bit2 == 1 && bit3 == 0 && bit5 == 1 && bit6 == 0)
            {
                _commandMotionData.Status = "Operation Enabled";
                UpdateSwitchOn(true);
            }
            else if (bit0 == 1 && bit1 == 1 && bit2 == 1 && bit3 == 0 && bit5 == 0 && bit6 == 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 = "Warning is occured";
            }

            if (bit4 == 0)
            {
                _commandMotionData.Status = "No Power";
            }

            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>
        /// Home
        /// </summary>
        public override bool Home()
        {
            bool result = base.Home();
            if(!result)
            {
                return false;
            }
            MotionData.IsHomed = false;
            _homeRoutine.Start(_homeTimeout, _axisConfig.HomingMethod);
            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>
        /// 首次启动写入COE线程
        /// </summary>
        public override void FirstStartUpWriteCOE(string variable)
        {
            _beckhoffCommonAxis.FirstStartUpWriteCOE(variable);
        }
        /// <summary>
        /// 启动写入COE线程
        /// </summary>
        public override void StartUpWriteCoeThread()
        {
            _beckhoffCommonAxis.StartUpWriteCoeThread();
        }
        /// <summary>
        /// 定时器
        /// </summary>
        /// <returns></returns>
        public override bool OnTimer()
        {
            lock (_operationLocker)
            {
                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)
                        {
                            if (_currentOperation == MotionOperation.Position)
                            {
                                _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)
            {
                LOG.WriteLog(eEvent.WARN_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 = _beckhoffCommonAxis.WriteControlWord(0x2F);
            if (!result)
            {
                return false;
            }
            result = BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.ProfileVelocity", speed);
            if (!result)
            {
                return false;
            }
            result = _beckhoffCommonAxis.WriteControlWord(0x3F);
            return result;
        }

        /// <summary>
        /// 改变速度加速度
        /// </summary>
        /// <param name="speed"></param>
        /// <returns></returns>
        public override bool ChangeSpeedAcceleration(int speed, int acceleration, int deceleration)
        {
            bool result = _beckhoffCommonAxis.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 = _beckhoffCommonAxis.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;
                    BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.{arg}", _profileVelocity);
                    break;
                case PROFILE_ACCEL:
                    _profileAcceleration = CalculateDivideAccelerationRatio(CalculateValueMultiplyScale(value));
                    _commandMotionData.FileAcceleration = value;
                    BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.{arg}", _profileAcceleration);
                    break;
                case PROFILE_DECEL:
                    _profileDeceleration =CalculateDivideAccelerationRatio(CalculateValueMultiplyScale(value));
                    _commandMotionData.FileDeceleration = value;
                    BeckhoffAxisManager.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;
            }
        }

        /// <summary>
        /// 初始化Axis配置
        /// </summary>
        /// <returns></returns>
        protected override AxisConfig InitializeAxisConfig()
        {
            AxisConfig axisConfig = BeckhoffAxisManager.Instance.GetAxis($"{Module}.{Name}");
            if (axisConfig != null)
            {
                BeckhoffAxis beckhoffAxis = axisConfig as BeckhoffAxis;
                _beckhoffCommonAxis.InitializeCoeOutputs(beckhoffAxis);
            }
            return axisConfig;
        }
        /// <summary>
        /// 订阅变量
        /// </summary>
        protected override void SubscriptionVariable()
        {
            _beckhoffCommonAxis.SubscriptionVariable();
        }
    }
}