PunkHPX8/PunkHPX8_RT/Devices/AXIS/BeckhoffCommonAxis.cs

using Aitex.Core.RT.Log;
using Aitex.Core.RT.SCCore;
using MECF.Framework.Common.Beckhoff.IOAxis;
using MECF.Framework.Common.CommonData.PUF;
using MECF.Framework.Common.TwinCat;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Reflection;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using static Mono.Security.X509.X520;

namespace PunkHPX8_RT.Devices.AXIS
{
    public class BeckhoffCommonAxis
    {
        #region 常量
        private const string STATUS_WORD = "StatusWord";
        private const string DIGITAL_INPUTS = "DigitalInputs";
        private const string CONTROL_WORD = "ControlWord";
        private const string MODE_OF_OPERATION = "ModeOfOperation";
        protected const string PROFILE_VELOCITY = "ProfileVelocity";
        protected const string PROFILE_ACCEL = "ProfileAccel";
        protected const string PROFILE_DECEL = "ProfileDecel";
        private const string REFERENCE_POSITION = "ReferencePosition";
        private const string TARGET_POSITION = "TargetPosition";
        private const string ACTUAL_VELOCITY = "Velocity";
        private const string ACTUAL_TORQUE = "ActualTorque";
        private const string POSITION_ERROR = "PositionError";
        private const string MOTOR_POSITION = "MotorPosition";
        private const string HOME_OFFSET = "HomeOffset";
        private const string HOMING_METHOD = "HomingMethod";
        protected const string HOMING_VELOCITY = "HomingVelocity";
        protected const string HOMING_VELOCITY_SLOW = "HomingVelocitySlow";
        protected const string HOMING_ACCEL = "HomingAccel";
        private const string NEGATIVE_TORQUE_LIMIT = "NegativeTorqueLimit";
        private const string POSITIVE_TORQUE_LIMIT = "PositiveTorqueLimit";
        private const string MANUF_STATUS = "ManufStatus";
        private const string SOFTWARE_LIMIT_MINUS = "SoftwareLimitMinus";
        private const string SOFTWARE_LIMIT_PLUS = "SoftwareLimitPlus";
        #endregion

        #region 内部变量
        /// <summary>
        /// 模块名称
        /// </summary>
        private string _module;
        /// <summary>
        /// 名称
        /// </summary>
        private string _name;
        /// <summary>
        /// 电机对象
        /// </summary>
        private JetAxisBase _axis;
        /// <summary>
        /// coe输出变量集合
        /// </summary>
        private List<string> _coeOutputs = new List<string>();
        /// <summary>
        /// coe连接状态
        /// </summary>
        private bool _coeConnected = false;
        /// <summary>
        /// beckhoff axis配置
        /// </summary>
        private BeckhoffAxis _beckhoffAxis;
        #endregion

        /// <summary>
        /// 初始化
        /// </summary>
        /// <param name="module"></param>
        /// <param name="name"></param>
        public BeckhoffCommonAxis(string module, string name,JetAxisBase axis) 
        {
            _module = module;
            _name = name;
            _axis = axis;
        }

        /// <summary>
        /// 初始化COE输出变量集合
        /// </summary>
        public void InitializeCoeOutputs(BeckhoffAxis beckhoffAxis)
        {
            _beckhoffAxis= beckhoffAxis;
            if (beckhoffAxis != null)
            {
                foreach (BeckhoffAxisOutput item in beckhoffAxis.Outputs)
                {
                    if (!_coeOutputs.Contains(item.Type) && item.Address.StartsWith("0x"))
                    {
                        _coeOutputs.Add(item.Type);
                    }
                }
            }
        }
        /// <summary>
        /// 首次写入COE
        /// </summary>
        public void FirstStartUpWriteCOE(string variable)
        {
            if (_coeOutputs.Contains(variable))
            {
                if (!_coeConnected)
                {
                    _coeConnected = true;
                    StartUpWriteCoeThread();
                }
            }
        }

        /// <summary>
        /// 启动写COE变量数值线程
        /// </summary>
        public void StartUpWriteCoeThread()
        {
            bool isSimulator = SC.GetValue<bool>("System.IsSimulatorMode");
            if (!isSimulator)
            {
                Thread thread = new Thread(new ThreadStart(StartUpWriteCoeVariable));
                thread.IsBackground = true;
                thread.Start();
            }
        }

        /// <summary>
        /// 启动写COE变量数值
        /// </summary>
        public void StartUpWriteCoeVariable()
        {
            LOG.WriteLog(eEvent.INFO_AXIS, _module, "start write coe thread");
            byte homingMethod = _beckhoffAxis.HomingMethod;
            WriteCoeVariable(HOMING_METHOD, homingMethod);
            //homing accel
            WriteCoeVariable(HOMING_ACCEL, _axis.CalculateDivideAccelerationRatio(_beckhoffAxis.HomingAccelDecel));
            //homing speed home switch
            WriteCoeVariable(HOMING_VELOCITY, _axis.CalculateMultiplySpeedRatio(_beckhoffAxis.HomingSpeedHomeSwitch));
            WriteCoeVariable(HOMING_VELOCITY_SLOW, _axis.CalculateMultiplySpeedRatio(_beckhoffAxis.HomingSpeedEncoderIndex));
            WriteCoeVariable(HOME_OFFSET, _beckhoffAxis.HomingOffset);
            int negativeTorqueLimit = _beckhoffAxis.NegativeTorqueLimit;
            if (negativeTorqueLimit != 0)
            {
                WriteCoeVariable(NEGATIVE_TORQUE_LIMIT, negativeTorqueLimit);
            }
            int positiveTorqueLimit = _beckhoffAxis.PositiveTorqueLimit;
            if (positiveTorqueLimit != 0)
            {
                WriteCoeVariable(POSITIVE_TORQUE_LIMIT, positiveTorqueLimit);
            }
            int softwareForwardLimit = _beckhoffAxis.ForwardSoftwareLimit;
            if (softwareForwardLimit != 0)
            {
                WriteCoeVariable(SOFTWARE_LIMIT_PLUS, softwareForwardLimit);
            }
            int softReverseLimit = _beckhoffAxis.ReverseSoftwareLimit;
            if (softReverseLimit != 0)
            {
                WriteCoeVariable(SOFTWARE_LIMIT_MINUS, softReverseLimit);
            }

            //WriteCoeVariable(PROFILE_VELOCITY,CalculateMultiplySpeedRatio(_beckhoffAxis.Speed));
            //WriteCoeVariable(PROFILE_ACCEL, CalculateDivideAccelerationRatio(_beckhoffAxis.Acceleration));
            //WriteCoeVariable(PROFILE_DECEL,CalculateDivideAccelerationRatio(_beckhoffAxis.Deceleration));
        }
        /// <summary>
        /// 写COE变量数值 
        /// </summary>
        /// <param name="variableName"></param>
        /// <param name="value"></param>
        private void WriteCoeVariable(string variableName, object value)
        {
            string str = $"{_module}.{_name}";
            if (_coeOutputs.Contains(variableName))
            {
                bool result = TwincatCoeManager.Instance.WriteVariableValue(str, variableName, value);
                if (result)
                {
                    LOG.WriteLog(eEvent.INFO_AXIS, str, $"coe variable {variableName} value {value} success");
                }
            }
        }
        /// <summary>
        /// 订阅变量
        /// </summary>
        public void SubscriptionVariable()
        {
            _axis.AxisSubscribeUpdateVariable(STATUS_WORD);
            _axis.AxisSubscribeUpdateVariable(DIGITAL_INPUTS);
            _axis.AxisSubscribeUpdateVariable(CONTROL_WORD);
            _axis.AxisSubscribeUpdateVariable(MODE_OF_OPERATION);
            _axis.AxisSubscribeUpdateVariable(PROFILE_VELOCITY);
            _axis.AxisSubscribeUpdateVariable(PROFILE_ACCEL);
            _axis.AxisSubscribeUpdateVariable(PROFILE_DECEL);
            _axis.AxisSubscribeUpdateVariable(REFERENCE_POSITION);
            _axis.AxisSubscribeUpdateVariable(TARGET_POSITION);
            _axis.AxisSubscribeUpdateVariable(MOTOR_POSITION);
            _axis.AxisSubscribeUpdateVariable(POSITION_ERROR);
            _axis.AxisSubscribeUpdateVariable(ACTUAL_TORQUE);
            _axis.AxisSubscribeUpdateVariable(ACTUAL_VELOCITY);
            _axis.AxisSubscribeUpdateVariable(HOME_OFFSET);
            _axis.AxisSubscribeUpdateVariable(HOMING_METHOD);
            _axis.AxisSubscribeUpdateVariable(HOMING_VELOCITY);
            _axis.AxisSubscribeUpdateVariable(HOMING_VELOCITY_SLOW);
            _axis.AxisSubscribeUpdateVariable(POSITIVE_TORQUE_LIMIT);
            _axis.AxisSubscribeUpdateVariable(NEGATIVE_TORQUE_LIMIT);
            _axis.AxisSubscribeUpdateVariable(HOMING_ACCEL);
            _axis.AxisSubscribeUpdateVariable(MANUF_STATUS);
        }

        /// <summary>
        /// 写入控制字
        /// </summary>
        /// <param name="controlWord"></param>
        public bool WriteControlWord(ushort controlWord)
        {
            return BeckhoffAxisManager.Instance.WriteVariableValue($"{_module}.{_name}.{CONTROL_WORD}", controlWord);
        }
        /// <summary>
        /// 写入ModeOfOperation
        /// </summary>
        /// <param name="modeOfOperation"></param>
        /// <returns></returns>
        public bool WriteModeOfMode(AxisModeOfOperation modeOfOperation)
        {
            return BeckhoffAxisManager.Instance.WriteVariableValue($"{_module}.{_name}.{MODE_OF_OPERATION}", (byte)modeOfOperation);
        }
        /// <summary>
        /// 写变量数值
        /// </summary>
        /// <param name="variableName"></param>
        /// <param name="value"></param>
        /// <returns></returns>
        public bool WriteVariable(string variableName, object value)
        {
            return BeckhoffAxisManager.Instance.WriteVariableValue($"{_module}.{_name}.{variableName}", value);
        }
    }
}