CyberX8/CyberX8_RT/Devices/AXIS/CanOpen/CanOpenAxis.cs

using Aitex.Core.RT.Event;
using Aitex.Core.RT.Fsm;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Util;
using MECF.Framework.Common.CommonData.PUF;
using MECF.Framework.Common.TwinCat;
using CyberX8_Core;
using CyberX8_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 CyberX8_RT.Devices.AXIS.CanOpen;

namespace CyberX8_RT.Devices.AXIS.CANOpen
{
    public class CanOpenAxis : JetAxisBase
    {
        #region 内部变量
        /// <summary>
        /// Home routine
        /// </summary>
        private CanOpenHomeRoutine _homeRoutine;
        /// <summary>
        /// 运动Routine
        /// </summary>
        private CanOpenProfilePositionRoutine _profilePositionRoutine;
        /// <summary>
        /// Switch On Routine
        /// </summary>
        private CanOpenSwitchOnRoutine _switchOnRoutine;
        /// <summary>
        /// Switch off Routine
        /// </summary>
        private CanOpenSwitchOffRoutine _switchOffRoutine;
        /// <summary>
        /// Stop Position
        /// </summary>
        private CanOpenStopPositionRoutine _stopPositionRoutine;
        #endregion
        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="Module"></param>
        public CanOpenAxis(string Module,string name):base(Module,name)
        {
        }
        /// <summary>
        /// 初始化参数
        /// </summary>
        protected override  void InitializeParameter()
        {
            _speedRatio = 10;
            _torqueRatio = 1000;
            _accelerationRatio = 1;
        }

        /// <summary>
        /// 初始化Routine
        /// </summary>
        protected override void InitializeRoutine()
        {
            _homeRoutine = new CanOpenHomeRoutine($"{Module}.{Name}", this);
            _profilePositionRoutine = new CanOpenProfilePositionRoutine($"{Module}.{Name}", this);
            _switchOnRoutine = new CanOpenSwitchOnRoutine($"{Module}.{Name}", this);
            _switchOffRoutine = new CanOpenSwitchOffRoutine($"{Module}.{Name}", this);
            _stopPositionRoutine = new CanOpenStopPositionRoutine($"{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)
            {
                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 = "Waring 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>
        /// 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>
        /// Home
        /// </summary>
        public override bool Home(bool isLogError)
        {
            bool result = base.Home(isLogError);
            if (!result)
            {
                return false;
            }
            _homeRoutine.Start(_homeTimeout, 0, isLogError);
            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>
        /// StopProfilePosition
        /// </summary>
        /// <returns></returns>
        
        /// <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;
                    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;
            }
        }
    }
}