CyberX8/CyberX8_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 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.Galil;
using MECF.Framework.Common.Device.Galil;
using MECF.Framework.Common.Utilities;

namespace CyberX8_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;
        #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;
        }
        /// <summary>
        /// 初始化Routine
        /// </summary>
        protected override void InitializeRoutine()
        {
            _homeRoutine = new GalilRotationHomeRoutine($"{Module}.{Name}", this);
            _profilePositionRoutine = new GalilRotationProfilePositionRoutine($"{Module}.{Name}", this);
            _switchOnRoutine = new GalilRotationSwitchOnRoutine($"{Module}.{Name}", this);
            _switchOffRoutine = new GalilRotationSwitchOffRoutine($"{Module}.{Name}", this);
            _stopPositionRoutine = new GalilRotationStopPositionRoutine($"{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)
        {
        }
        /// <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;
            }
            MotionData.IsHomed = false;
            _homeRoutine.Start(_homeTimeout, _galilAxisConfig.HomingAcceleration,_galilAxisConfig.HomingDeceleration,_galilAxisConfig.HomingSpeed,_galilAxisConfig.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 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)
            {
                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 = 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;
                    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;
            }
        }
    }
}