PunkHPX8/PunkHPX8_RT/Devices/AXIS/GalilRotation/GalilRotationAxis.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.Galil;
using MECF.Framework.Common.Device.Galil;
using MECF.Framework.Common.Utilities;
using MECF.Framework.Common.Device;
using static Mono.Security.X509.X520;

namespace PunkHPX8_RT.Devices.AXIS.GalilLipsel
{
    public class GalilRotationAxis : JetAxisBase
    {
        #region 内部变量
        /// <summary>
        /// Home routine
        /// </summary>
        private GalilRotationHomeRoutine _homeRoutine;
        /// <summary>
        /// 运动Routine
        /// </summary>
        private GalilRotationProfilePositionRoutine _profilePositionRoutine;
        /// <summary>
        /// Switch On Routine
        /// </summary>
        private GalilRotationSwitchOnRoutine _switchOnRoutine;
        /// <summary>
        /// Switch Off Routine
        /// </summary>
        private GalilRotationSwitchOffRoutine _switchOffRoutine;
        /// <summary>
        /// Stop Position
        /// </summary>
        private GalilRotationStopPositionRoutine _stopPositionRoutine;
        /// <summary>
        /// Axis Operation Locker
        /// </summary>
        private readonly object _operationLocker = new object();

        /// <summary>
        /// Galil共用对象
        /// </summary>
        private GalilCommonAxis _galilCommoAxis;
        #endregion
        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="Module"></param>
        public GalilRotationAxis(string Module, string name) : base(Module, name)
        {
            //deg->RPM
            _commandMotionData.FileProfileVelocity /= 6;
            _commandMotionData.FileHomingVelocity /= 6;
        }
        /// <summary>
        /// 初始化参数
        /// </summary>
        protected override void InitializeParameter()
        {
            _accelerationRatio = 1;
            _speedRatio = 6;
            _galilCommoAxis = new GalilCommonAxis(Module, Name, this);
        }
        /// <summary>
        /// 初始化Routine
        /// </summary>
        protected override void InitializeRoutine()
        {
            _homeRoutine = new GalilRotationHomeRoutine($"{Module}.{Name}", this, _galilCommoAxis);
            _profilePositionRoutine = new GalilRotationProfilePositionRoutine($"{Module}.{Name}", this, _galilCommoAxis);
            _switchOnRoutine = new GalilRotationSwitchOnRoutine($"{Module}.{Name}", this, _galilCommoAxis);
            _switchOffRoutine = new GalilRotationSwitchOffRoutine($"{Module}.{Name}", this,_galilCommoAxis);
            _stopPositionRoutine = new GalilRotationStopPositionRoutine($"{Module}.{Name}", this, _galilCommoAxis);
        }
        /// <summary>
        /// 中止操作
        /// </summary>
        /// <param name="cmd"></param>
        /// <param name="args"></param>
        /// <returns></returns>
        public override bool StopPositionOperation()
        {
            if (!IsRun)
            {
                return true;
            }
            lock (_operationLocker)
            {
                _currentOperation = MotionOperation.StopPosition;              
                if (_profilePositionRoutine.Monitor() == RState.Running)
                {
                    _profilePositionRoutine.Abort();
                }
                _status = _stopPositionRoutine.Start();
            } 
                     
            return true;
        }

        /// <summary>
        /// 更新StatusWord
        /// </summary>
        /// <param name="status"></param>
        public override void UpdateStatusWord(ushort status)
        {
        }
        /// <summary>
        /// Home
        /// </summary>
        public override bool Home()
        {
            bool result = base.Home();
            if (!result)
            {
                return false;
            }
            MotionData.IsHomed = false;
            _homeRoutine.Start(_homeTimeout, _axisConfig.HomingAcceleration,_axisConfig.HomingDeceleration,_axisConfig.HomingSpeed,_axisConfig.HomingOffset);
            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>
        /// 开始运动 
        /// </summary>
        /// <returns></returns>
        public override bool StartMotion()
        {
            return _galilCommoAxis.WriteStartMotion();
        }
        /// <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)
        {
            return _galilCommoAxis.WriteSpeed(speed);
        }

        /// <summary>
        /// 改变速度加速度
        /// </summary>
        /// <param name="speed"></param>
        /// <returns></returns>
        public override bool ChangeSpeedAcceleration(int speed, int acceleration, int deceleration)
        {
            bool result = _galilCommoAxis.WriteSpeed(speed);
            if (result)
            {
                result = _galilCommoAxis.WriteAcceleration(acceleration);
                if (result)
                {
                    result = _galilCommoAxis.WriteDeceleration(deceleration);
                }
            }
            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;
                    GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "SP", _profileVelocity);
                    break;
                case PROFILE_ACCEL:
                    _profileAcceleration = CalculateDivideAccelerationRatio(CalculateValueMultiplyScale(value));
                    _commandMotionData.FileAcceleration = value; 
                    GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "AC", _profileAcceleration);
                    break;
                case PROFILE_DECEL:
                    _profileDeceleration = CalculateDivideAccelerationRatio(CalculateValueMultiplyScale(value));
                    _commandMotionData.FileDeceleration = value;
                    GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "DC", _profileDeceleration);
                    break;
                case HOMING_VELOCITY:
                    _profileHomingVelocity = CalculateMultiplySpeedRatio(CalculateValueMultiplyScale(value));
                    _commandMotionData.FileHomingVelocity = value;
                    GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "SP", _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;
                    GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "AC", _profileHomingAccel);
                    break;
            }
        }

        /// <summary>
        /// 初始化Axis配置
        /// </summary>
        /// <returns></returns>
        protected override AxisConfig InitializeAxisConfig()
        {
            return GalilControllerCfgManager.Instance.GetGalilAxisConfig(Module.ToString(), Name);
        }
        /// <summary>
        /// 订阅变量
        /// </summary>
        protected override void SubscriptionVariable()
        {
            _galilCommoAxis.SubscriptionVariable();
        }
    }
}