CyberX8/CyberX8_RT/Devices/PUF/PufDistanceSensor.cs

using Aitex.Core.RT.DataCenter;
using Aitex.Core.RT.Device;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.OperationCenter;
using Aitex.Core.RT.SCCore;
using MECF.Framework.Common.Beckhoff.AxisProvider;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.TwinCat;
using MECF.Framework.RT.Core.Equipments;
using CyberX8_RT.Devices.AXIS;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using MECF.Framework.Common.IOCore;

namespace CyberX8_RT.Devices.PUF
{
    public class PufDistanceSensor : BaseDevice, IDevice
    {
        #region 常量
        private const string STUCK_WAFER_300_OFFSET = "CheckStuckWafer300_Offset_mm";
        private const string STUCK_WAFER_300_DISTANCE_OFFSET1 = "CheckStuckWafer300_DistanceOffset1";
        private const string STUCK_WAFER_300_DISTANCE_OFFSET2 = "CheckStuckWafer300_DistanceOffset2";
        private const string STUCK_WAFER_300_ACCELERATION = "CheckStuckWafer300_Acceleration_mm_sec_sec";
        private const string STUCK_WAFER_300_DECELERATION = "CheckStuckWafer300_Deceleration_mm_sec_sec";
        private const string STUCK_WAFER_300_SPEED = "CheckStuckWafer300_Speed_mm_sec";
        private const string STUCK_WAFER_300_TORQUE_LIMIT = "CheckStuckWafer300_TorqueLimit_Percent";
        private const string STUCK_WAFER_200_OFFSET = "CheckStuckWafer200_Offset_mm";
        private const string STUCK_WAFER_200_DISTANCE_OFFSET1 = "CheckStuckWafer200_DistanceOffset1";
        private const string STUCK_WAFER_200_DISTANCE_OFFSET2 = "CheckStuckWafer200_DistanceOffset2";
        private const string STUCK_WAFER_200_ACCELERATION = "CheckStuckWafer200_Acceleration_mm_sec_sec";
        private const string STUCK_WAFER_200_DECELERATION = "CheckStuckWafer200_Deceleration_mm_sec_sec";
        private const string STUCK_WAFER_200_SPEED = "CheckStuckWafer200_Speed_mm_sec";
        private const string STUCK_WAFER_200_TORQUE_LIMIT = "CheckStuckWafer200_TorqueLimit_Percent";

        private const string DISTANCE_SENSOR1="DistanceSensor1";
        private const string DISTANCE_SENSOR2 = "DistanceSensor2";
        private const string DISTANCE_OFFSET1 = "DistanceOffset1";
        private const string DISTANCE_OFFSET2 = "DistanceOffset2";

        private const double STANDARD = 100;
        #endregion

        #region 内部变量
        //动态变量主要是用于calibrate显示
        private double _dynamicOffset1 = 0;
        private double _dynamicOffset2 = 0;
        //执行过程中主要用于以下两个变量
        private double _offset1 = 0;
        private double _offset2 = 0;
        /// <summary>
        /// 尺寸
        /// </summary>
        private int _waferSize = 200;
        /// <summary>
        /// 加速度
        /// </summary>
        private double _acceleration = 0;
        /// <summary>
        /// 减速度
        /// </summary>
        private double _deceleration = 0;
        /// <summary>
        /// 速度
        /// </summary>
        private double _speed = 0;
        /// <summary>
        /// 距离
        /// </summary>
        private double _distance = 0;
        /// <summary>
        /// 比例因子
        /// </summary>
        private double _verticalScaleFactor = 0;
        /// <summary>
        /// 垂直轴
        /// </summary>
        private JetAxisBase _verticalAxis;
        /// <summary>
        /// distance sensor1
        /// </summary>
        private double _distanceSensor1;
        /// <summary>
        /// distance sensor2
        /// </summary>
        private double _distanceSensor2;
        /// <summary>
        /// 
        /// </summary>
        private double _torquelimit;
        /// <summary>
        /// Distance sensor阈值
        /// </summary>
        private double _checkStuckWafer_DistanceSensorThreshold_mm;
        #endregion
        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="moduleName"></param>
        public PufDistanceSensor(string moduleName): base(moduleName,"DistanceSensor", "DistanceSensor", "DistanceSensor")
        {
            _waferSize = SC.GetValue<int>("System.WaferSize");
            BeckhoffProviderAxis verticalBeckhoffProviderAxis = BeckhoffAxisProviderManager.Instance.GetAxisProvider($"{Module}.Vertical");
            if (verticalBeckhoffProviderAxis != null)
            {
                _verticalScaleFactor = verticalBeckhoffProviderAxis.ScaleFactor;
            }
            LoadConfigParameter();
            _dynamicOffset1 = _offset1;
            _dynamicOffset2 = _offset2;
            _verticalAxis = DEVICE.GetDevice<JetAxisBase>($"{moduleName}.Vertical");
        }
        /// <summary>
        /// 初始化
        /// </summary>
        /// <returns></returns>
        public bool Initialize()
        {
            InitializeData();
            SubscribeValueAction();
            return true;
        }
        /// <summary>
        /// 订阅变量数值发生变化
        /// </summary>
        private void SubscribeValueAction()
        {
            IOModuleManager.Instance.SubscribeModuleVariable($"{Module}", DISTANCE_SENSOR1, UpdateVariableValue);
            IOModuleManager.Instance.SubscribeModuleVariable($"{Module}", DISTANCE_SENSOR2, UpdateVariableValue);
        }
        /// 更新变量数值
        /// </summary>
        /// <param name="variable"></param>
        /// <param name="value"></param>
        private void UpdateVariableValue(string variable, object value)
        {
            if (variable == $"{DISTANCE_SENSOR1}")
            {
                _distanceSensor1 = (double)value;
            }
            else if (variable == $"{DISTANCE_SENSOR2}")
            {
                _distanceSensor2 = (double)value;
            }
        }
        /// <summary>
        /// 初始化数据
        /// </summary>
        private void InitializeData()
        {
            DATA.Subscribe($"{Module}.{DISTANCE_OFFSET1}", () => _dynamicOffset1, Aitex.Core.Util.SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{DISTANCE_OFFSET2}", () => _dynamicOffset2, Aitex.Core.Util.SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{DISTANCE_SENSOR1}", () => _distanceSensor1, Aitex.Core.Util.SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{DISTANCE_SENSOR2}", () => _distanceSensor2, Aitex.Core.Util.SubscriptionAttribute.FLAG.IgnoreSaveDB);
        }
        /// <summary>
        /// 保存参数
        /// </summary>
        /// <param name="offset1"></param>
        /// <param name="offset2"></param>
        public void SaveConfig(double offset1,double offset2)
        {
            _dynamicOffset1=offset1;
            _dynamicOffset2=offset2;
            if(_waferSize==200)
            {
                SC.SetItemValue($"{Module}.{STUCK_WAFER_200_DISTANCE_OFFSET1}", offset1);
                SC.SetItemValue($"{Module}.{STUCK_WAFER_200_DISTANCE_OFFSET2}", offset2);
            }
            else
            {
                SC.SetItemValue($"{Module}.{STUCK_WAFER_300_DISTANCE_OFFSET1}", offset1);
                SC.SetItemValue($"{Module}.{STUCK_WAFER_300_DISTANCE_OFFSET2}", offset2);
            }
        }
        /// <summary>
        /// Stick Distance
        /// </summary>
        public bool GotoStickDistance()
        {
            LoadConfigParameter();
            return _verticalAxis.ProfilePosition(GetAfterScaleData(_verticalAxis.MotionData.MotorPosition + _distance),GetAfterScaleData(_speed),
                GetAfterScaleData(_acceleration),GetAfterScaleData(_deceleration),true);
        }
        /// <summary>
        /// 获取尺寸放大后的数据
        /// </summary>
        /// <param name="value"></param>
        /// <returns></returns>
        private int GetAfterScaleData(double value)
        {
            return (int)Math.Round(value * _verticalScaleFactor, 0);
        }
        /// <summary>
        /// Stick Distance检验结果
        /// </summary>
        /// <returns></returns>
        public bool CheckStickDistanceStatus()
        {
            LoadConfigParameter();
            double inter1 = STANDARD - _distanceSensor1;
            double inter2=STANDARD - _distanceSensor2;
            if(_verticalAxis.MotionData.ActualTorque>_torquelimit)
            {
                LOG.WriteLog(eEvent.ERR_PUF, Module, $"Torque {_verticalAxis.MotionData.ActualTorque} is over {_torquelimit}");
                return false;
            }
            double offsetInter1=Math.Abs(_offset1- inter1);
            double offsetInter2=Math.Abs(_offset2- inter2);
            if(offsetInter1>_checkStuckWafer_DistanceSensorThreshold_mm)
            {
                LOG.WriteLog(eEvent.ERR_PUF, Module, $"offset bias {offsetInter1} is over {_checkStuckWafer_DistanceSensorThreshold_mm}");
                return false;
            }
            if (offsetInter2 > _checkStuckWafer_DistanceSensorThreshold_mm)
            {
                LOG.WriteLog(eEvent.ERR_PUF, Module, $"offset bias {offsetInter2} is over {_checkStuckWafer_DistanceSensorThreshold_mm}");
                return false;
            }
            return true;
        }

        private void LoadConfigParameter()
        {
            if (_waferSize == 200)
            {
                _distance = SC.GetValue<double>($"{Module}.{STUCK_WAFER_200_OFFSET}");
                _offset1 = SC.GetValue<double>($"{Module}.{STUCK_WAFER_200_DISTANCE_OFFSET1}");
                _offset2 = SC.GetValue<double>($"{Module}.{STUCK_WAFER_200_DISTANCE_OFFSET2}");
                _acceleration = SC.GetValue<double>($"{Module}.{STUCK_WAFER_200_ACCELERATION}");
                _deceleration = SC.GetValue<double>($"{Module}.{STUCK_WAFER_200_DECELERATION}");
                _speed = SC.GetValue<double>($"{Module}.{STUCK_WAFER_200_SPEED}");
                _torquelimit = SC.GetValue<double>($"{Module}.{STUCK_WAFER_200_TORQUE_LIMIT}");
            }
            else
            {
                _distance = SC.GetValue<double>($"{Module}.{STUCK_WAFER_300_OFFSET}");
                _offset1 = SC.GetValue<double>($"{Module}.{STUCK_WAFER_300_DISTANCE_OFFSET1}");
                _offset2 = SC.GetValue<double>($"{Module}.{STUCK_WAFER_300_DISTANCE_OFFSET2}");
                _acceleration = SC.GetValue<double>($"{Module}.{STUCK_WAFER_300_ACCELERATION}");
                _deceleration = SC.GetValue<double>($"{Module}.{STUCK_WAFER_300_DECELERATION}");
                _speed = SC.GetValue<double>($"{Module}.{STUCK_WAFER_300_SPEED}");
                _torquelimit = SC.GetValue<double>($"{Module}.{STUCK_WAFER_300_TORQUE_LIMIT}");
            }

            if (SC.ContainsItem($"{Module}.CheckStuckWafer_DistanceSensorThreshold_mm"))
            {
                _checkStuckWafer_DistanceSensorThreshold_mm = SC.GetValue<double>($"{Module}.CheckStuckWafer_DistanceSensorThreshold_mm");
            }
        }
        public void Monitor()
        {
        }

        public void Reset()
        {
        }

        public void Terminate()
        {
        }
    }
}