CyberX8/Framework/Common/Simulator/MotorSimulator.cs

using Aitex.Common.Util;
using Aitex.Core.RT.Device;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.Routine;
using Aitex.Core.Util;
using CyberX8_Core;
using MECF.Framework.Common.Beckhoff.AxisProvider;
using MECF.Framework.Common.Beckhoff.Station;
using MECF.Framework.Common.Device.Galil;
using MECF.Framework.Common.Equipment;
using System;
using System.Collections.Generic;
using System.IO;
using System.Runtime.InteropServices.ComTypes;

namespace MECF.Framework.Common.Simulator
{
    
    /// <summary>
    /// 电机运动模拟器
    /// </summary>
    public class MotorSimulator : Singleton<MotorSimulator>
    {
        
        #region 常量
        private const string TARGET_VELOCITY = "TargetVelocity";
        private const string TARGET_ACCEL = "TargetAcceleration";
        private const string TARGET_DECEL = "TargetDeceleration";
        private const string TARGET_POSITION = "TargetPosition";
        private const string SWITCH_SIGNAL = "SwitchSignal";
        private const string ACTUAL_POSITION = "ActualPosition";
        private const string AUXILIARY_POSITION = "AuxiliaryPosition";
        private const string HOMING_SIGNAL = "HomingSignal";
        private const string MOTION_SIGNAL = "MotionSignal";
        private const string STOP_SIGNAL = "StopSignal";
        private const string FESTO_DATABUFFER_TRANSPORT1_LOCK = "ProcessTransporterLock";
        private const string FESTO_DATABUFFER_TRANSPORT2_LOCK = "LoaderTransporterLock";
        /// <summary>
        /// 定时器间隔(ms)
        /// </summary>
        private const int TIMER_INTERVAL = 50;
        /// <summary>
        /// motor step factor
        /// </summary>
        private const int MOTOR_STEP_FACTOR = 10;
        #endregion

        #region 内部变量
        /// <summary>
        /// 定时器
        /// </summary>
        private PeriodicJob _periodicJob;      
        /// <summary>
        /// 电机数据字典(key:Name(module.name)，value:Datas)
        /// </summary>
        private Dictionary<string, SimulatorMotionData> _motorNameDataDic = new Dictionary<string, SimulatorMotionData>();
        /// <summary>
        /// Key:moduleName, Value:minStep
        /// </summary>
        private Dictionary<string, int> _motorNameMinStepDic = new Dictionary<string, int>();
        /// <summary>
        /// Key:stationName, Value:position
        /// </summary>
        private Dictionary<string, double> _stationPositionDic = new Dictionary<string, double>();
        /// <summary>
        /// Key:stationName, Value:Tolerance
        /// </summary>
        private Dictionary<string, double> _toleranceDic = new Dictionary<string, double>();
        /// <summary>
        /// Key:stationName, Value:BeckhoffProviderAxis
        /// </summary>
        private Dictionary<string, BeckhoffProviderAxis> _axisDataDic = new Dictionary<string, BeckhoffProviderAxis>();
        /// <summary>
        /// 其他模块数据
        /// </summary>
        private Dictionary<string, bool> _otherModuleDataDic = new Dictionary<string, bool>();
        #endregion

        #region 属性

        #endregion

        //delegate
        #region Delegate
        public delegate void UpdateVariableValueMotionDatasChanged(Dictionary<string, SimulatorMotionData> datasDic);
        public delegate void UpdateVariableValueInputDatasChanged(string module, string VariableName, bool value);
        #endregion

        #region 事件
        /// <summary>
        /// MotionDatas变更事件
        /// </summary>
        public event UpdateVariableValueMotionDatasChanged OnUpdateMotionDatasChanged;
        /// <summary>
        /// InputDatas变更事件
        /// </summary>
        public event UpdateVariableValueInputDatasChanged OnUpdateInputDatasChanged;
        #endregion
        /// <summary>
        /// 初始化
        /// </summary>
        public void Initialize()
        {
            _periodicJob = new PeriodicJob(TIMER_INTERVAL, OnTimer, "Motor Simulator Timer", true);
            Init();
        }
        /// <summary>
        /// 初始化数据
        /// </summary>
        private void Init()
        {
            SimulatorCommManager.Instance.OnUpdateVariableValueChanged += UpdataDataCausedByOtherModule;
            InitOtherModuleDatas();
            //加载对应配置文件 GalilControllerCfg-Simulator.xml，初始化数据字典
            try
            {
                string oldXmlPath = PathManager.GetCfgDir();
                string newXmlPath = oldXmlPath.Replace("CyberX8_Simulator", "CyberX8_RT") + "Devices\\GalilControllerCfg-Simulator.xml";
                GalilControllerCfg cfg = CustomXmlSerializer.Deserialize<GalilControllerCfg>(new FileInfo(newXmlPath));
                if (cfg != null)
                {
                    foreach (GalilDeviceConfig config in cfg.GalilDeviceConfigs)
                    {
                        foreach (GalilAxisConfig item in config.GalilAxises)
                        {
                            _motorNameDataDic[$"{config.Module}.{item.Name}"] = new SimulatorMotionData();
                            _motorNameDataDic[$"{config.Module}.{item.Name}"].FwdSoftLimit = item.ForwardSoftwareLimit;
                            _motorNameDataDic[$"{config.Module}.{item.Name}"].RevSoftLimit = item.ReverseSoftwareLimit;
                            _motorNameDataDic[$"{config.Module}.{item.Name}"].NegativeTorqueLimit = item.NegativeTorqueLimit;
                            _motorNameDataDic[$"{config.Module}.{item.Name}"].PositiveTorqueLimit = item.PositiveTorqueLimit;
                            _motorNameDataDic[$"{config.Module}.{item.Name}"].SwitchSignal = true;
                            _motorNameDataDic[$"{config.Module}.{item.Name}"].ModuleName = config.Module;
                        }
                    }
                }
            }
            catch
            {
                LOG.WriteLog(eEvent.ERR_GALIL, "Galil", "Load galil GalilControllerCfg-Simulator.xml failed");
            }                  
            
            //加载AxisProviderCfg.xml
            try
            {
                string oldXmlPath = PathManager.GetCfgDir();
                string newXmlPath = oldXmlPath.Replace("CyberX8_Simulator", "CyberX8_RT") + "Devices\\AxisProviderCfg.xml";
                BeckhoffAxisProviderCfg axisProviderCfg = CustomXmlSerializer.Deserialize<BeckhoffAxisProviderCfg>(new FileInfo(newXmlPath));
                if (axisProviderCfg != null)
                {
                    foreach (BeckhoffProviderAxis item in axisProviderCfg.Axes)
                    {
                        double value = item.ScaleFactor / 30;
                        _axisDataDic[item.Name] = item;
                        _motorNameMinStepDic[item.Name] = (int)((value < 100) ? 100 : value);                        
                    }
                }
            }
            catch
            {
                LOG.WriteLog(eEvent.ERR_AXIS, "axisProvider", "Load AxisProviderCfg.xml failed");
            }
            //加载对应配置文件 StationPositionCfg-Simulator.xml，初始化数据字典
            try
            {
                string oldXmlPath = PathManager.GetCfgDir();
                string newXmlPath = oldXmlPath.Replace("CyberX8_Simulator", "CyberX8_RT") + "Station\\StationPositionsCfg_Simulator.xml";
                StationPositionCfg cfg = CustomXmlSerializer.Deserialize<StationPositionCfg>(new FileInfo(newXmlPath));
                if (cfg != null)
                {
                    foreach (BeckhoffStationModule config in cfg.Module)
                    {
                        if (config.Name.Contains("Transporter"))
                        {
                            foreach (BeckhoffStationAxis item in config.Axises)
                            {
                                _toleranceDic[item.Name] = item.ToleranceDefault;
                                if (item.Name.Contains("Elevator"))
                                {
                                    foreach(var subItem in item.Stations)
                                    {
                                        _stationPositionDic[subItem.Name.ToLower()] = double.Parse(subItem.Position);
                                    }                                   
                                }
                            }
                        }                        
                    }
                }
            }
            catch
            {
                LOG.WriteLog(eEvent.ERR_GALIL, "StationPosition", "Load StationPositionCfg-Simulator.xml failed");
            }
        }
        private void InitOtherModuleDatas()
        {
            _otherModuleDataDic[FESTO_DATABUFFER_TRANSPORT1_LOCK] = false;
            _otherModuleDataDic[FESTO_DATABUFFER_TRANSPORT2_LOCK] = false;
        }
        /// <summary>
        /// 定时器执行
        /// </summary>
        /// <returns></returns>
        private bool OnTimer()
        {
            //电机运动模型
            foreach(var motorItem in _motorNameDataDic)
            {
                //对应电机进行模拟
                MotorMotionSimulator(motorItem);
                //实时更新电机数据
                UpdateMotionDatas(_motorNameDataDic);
                //更新InputDatas
                UpdateInputDatas(motorItem);                              
            }
            
            return true;
        }
        /// <summary>
        /// 通知Galil模块motion数据变化
        /// </summary>
        /// <param name="data"></param>
        private void UpdateMotionDatas(Dictionary<string, SimulatorMotionData> datasDic)
        {
            if (OnUpdateMotionDatasChanged != null)
            {
                OnUpdateMotionDatasChanged(datasDic);
            }
        }
        /// <summary>
        /// 通知Galil模块Input数据变化
        /// </summary>
        /// <param name="module"></param>
        /// <param name="value"></param>
        private void UpdateInputDatas(KeyValuePair<string, SimulatorMotionData> motorItem)
        {         
            if (OnUpdateInputDatasChanged != null && CheckMotionData(motorItem, out string VariableName, out bool value))
            {
                OnUpdateInputDatasChanged(motorItem.Value.ModuleName, VariableName, value);
            }
        }

        /// <summary>
        /// 设置电机数据
        /// </summary>
        /// <param name="axisName"></param>
        /// <param name="type"></param>
        /// <param name="value"></param>
        public void SetMotionData(string axisName, string type, object value)
        {
            switch (type)
            {
                case TARGET_VELOCITY:
                    _motorNameDataDic[axisName].TargetVelocity = (int)value;
                    break;
                case TARGET_ACCEL:
                    _motorNameDataDic[axisName].TargetAccel = (int)value;
                    break;
                case TARGET_DECEL:
                    _motorNameDataDic[axisName].TargetDecel = (int)value;
                    break;
                case TARGET_POSITION:
                    _motorNameDataDic[axisName].TargetPosition = (int)value;
                    break;
                case ACTUAL_POSITION:
                    _motorNameDataDic[axisName].ActualPosition = (int)value;
                    break;
                case SWITCH_SIGNAL:
                    _motorNameDataDic[axisName].SwitchSignal = (bool)value;
                    break;
                case STOP_SIGNAL:
                    _motorNameDataDic[axisName].StopSignal = true;
                    break;               
                case HOMING_SIGNAL:
                    _motorNameDataDic[axisName].HomingSignal = true;
                    break;
                case MOTION_SIGNAL:
                    _motorNameDataDic[axisName].MotionSignal = true;
                    break;
                case AUXILIARY_POSITION:
                    //++
                    break;
                default:
                    break;
            }
        }
        /// <summary>
        /// 运动模拟器
        /// </summary>
        private void MotorMotionSimulator(KeyValuePair<string, SimulatorMotionData> motor)
        {
            SimulatorMotionData motionData = motor.Value;
            string name = motor.Key;
            //上电检查
            if (!motionData.SwitchSignal) return;
            if (motionData.HomingSignal)
            {
                HomeOperation(motionData);
            }
            else
            {
                PositionOperation(motionData, name);
            }            
        }
        
        /// <summary>
        /// Home操作
        /// </summary>
        /// <param name="data"></param>
        private void HomeOperation(SimulatorMotionData motionData)
        {
            if (motionData.MotionSignal)
            {
                motionData.StopCode = 10;//HM操作停止码10
                motionData.MotionSignal = false;
                motionData.HomingSignal = false;
                motionData.MotionPhase = MotionPhase.Accelerating;
                //motionData.ActualPosition = 0;
                motionData.ActualVelocity = 0;
            }            
        }
        /// <summary>
        /// GoToPosition操作
        /// </summary>
        /// <param name="data"></param>
        private void PositionOperation(SimulatorMotionData motionData, string name)
        {
            if (motionData.MotionSignal)
            {
                //正向运动
                int motorStep = Math.Abs(motionData.ActualPosition - motionData.TargetPosition);
                //motionData.ActualVelocity = TrapezoidalSpeedControl(motionData);
                motionData.ActualVelocity = motionData.TargetVelocity;
                if (motionData.ActualPosition < motionData.TargetPosition)
                {
                    //motionData.ActualPosition += (motionData.ActualVelocity * TIMER_INTERVAL / 1000);
                    motionData.ActualPosition += ((motorStep / MOTOR_STEP_FACTOR < _motorNameMinStepDic[name]) ? _motorNameMinStepDic[name] : motorStep / MOTOR_STEP_FACTOR);
                    bool fwdLimit = motionData.FwdSoftLimit != 0 ? motionData.ActualPosition >= motionData.FwdSoftLimit : false;
                    if (fwdLimit || motionData.ActualPosition >= motionData.TargetPosition)
                    {
                        motionData.StopCode = 1;//正常运动停止码1
                        motionData.MotionSignal = false;
                        //motionData.MotionPhase = MotionPhase.Accelerating;
                        motionData.ActualPosition = fwdLimit ? motionData.FwdSoftLimit : motionData.TargetPosition;
                        motionData.ActualVelocity = 0;
                    }
                }
                //反向运动
                else if (motionData.ActualPosition > motionData.TargetPosition)
                {
                    motionData.ActualPosition -= ((motorStep / MOTOR_STEP_FACTOR < _motorNameMinStepDic[name]) ? _motorNameMinStepDic[name] : motorStep / MOTOR_STEP_FACTOR);
                    //motionData.ActualPosition -= (motionData.ActualVelocity * TIMER_INTERVAL / 1000);
                    bool revLimit = motionData.RevSoftLimit != 0 ? motionData.ActualPosition <= motionData.RevSoftLimit : false;
                    if (revLimit || motionData.ActualPosition <= motionData.TargetPosition)
                    {
                        motionData.StopCode = 1;//正常运动停止码1
                        motionData.MotionSignal = false;
                        //motionData.MotionPhase = MotionPhase.Accelerating;
                        motionData.ActualPosition = revLimit ? motionData.RevSoftLimit : motionData.TargetPosition;
                        motionData.ActualVelocity = 0;
                    }
                }
            }

            //停止信号
            if (motionData.StopSignal)
            {
                motionData.MotionSignal = false;
                motionData.StopCode = 4;//ST操作停止码4
                motionData.StopSignal = false;
            }
        }
        /// <summary>
        /// 梯形加减速运动控制
        /// </summary>
        private int TrapezoidalSpeedControl(SimulatorMotionData data)
        {
            int speed = 0;
            //制动距离
            int brakeDistance = (data.ActualVelocity * data.ActualVelocity) / (2 * data.TargetDecel);
            int remainingDistance = Math.Abs(data.ActualPosition - data.TargetPosition);
            if (brakeDistance >= remainingDistance)
            {
                data.MotionPhase = MotionPhase.Decelerating;
            }
            else if (data.ActualVelocity < data.TargetVelocity && data.MotionPhase != MotionPhase.Decelerating)
            {
                data.MotionPhase = MotionPhase.Accelerating;
            }
            else
            {
                data.MotionPhase = MotionPhase.ConstantSpeed;
            }

            // 速度更新
            switch (data.MotionPhase)
            {
                case MotionPhase.Accelerating:
                    speed = Math.Min(data.ActualVelocity + data.TargetAccel * TIMER_INTERVAL / 1000, data.TargetVelocity);
                    break;
                case MotionPhase.ConstantSpeed:
                    speed = data.TargetVelocity;
                    break;
                case MotionPhase.Decelerating:
                    //speed = Math.Max(data.ActualVelocity - data.TargetDecel * TIMER_INTERVAL / 1000, 10);
                    speed = Math.Max(data.ActualVelocity / 100, 10);
                    break;
            }
            return speed;
        }
        /// <summary>
        /// 检查电机数据
        /// </summary>
        /// <returns></returns>
        private bool CheckMotionData(KeyValuePair<string, SimulatorMotionData> motor, out string name,out bool value)
        {
            bool result = false;
            value = false;
            name = "";
            switch (motor.Key) 
            {
                case "Transporter1.Elevator":
                    name = "r_TRANSPORT1_WS_HOLD_PRESENT";
                    value = _otherModuleDataDic[FESTO_DATABUFFER_TRANSPORT1_LOCK] && CheckAtStation(motor.Key, motor.Value, "up") ? true : false; 
                    result = true;
                    break;
                case "Transporter2.Elevator":
                    name = "r_TRANSPORT2_WS_HOLD_PRESENT";
                    value = _otherModuleDataDic[FESTO_DATABUFFER_TRANSPORT2_LOCK] && CheckAtStation(motor.Key, motor.Value, "up") ? true : false;
                    result = true;
                    break;
                default:
                    break;
            }  
            return result;
        }
        /// <summary>
        /// 更新其他模块数据
        /// </summary>
        /// <param name="name"></param>
        /// <param name="value"></param>
        private void UpdataDataCausedByOtherModule(string name, bool value)
        {
            if (_otherModuleDataDic.ContainsKey(name))
            {
                _otherModuleDataDic[name] = value;
            }
        }
        private bool CheckAtStation(string name, SimulatorMotionData data, string stationName)
        {
            double diff = Math.Abs(data.ActualPosition - _stationPositionDic[$"{name.ToLower()}.{stationName.ToLower()}"] * _axisDataDic[name].ScaleFactor);
            if (diff <= _toleranceDic[name] * _axisDataDic[name].ScaleFactor)
            {
                return true;
            }
            return false;
        }
    }
}