CyberX8/Framework/Common/Simulator/MotorSimulator.cs

using Aitex.Common.Util;
using Aitex.Core.RT.Log;
using Aitex.Core.Util;
using MECF.Framework.Common.Beckhoff.AxisProvider;
using MECF.Framework.Common.Device.Galil;
using System;
using System.Collections.Generic;
using System.IO;

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";
        /// <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>();
        #endregion

        #region 属性

        #endregion

        //delegate
        #region Delegate
        public delegate void UpdateVariableValueChanged(Dictionary<string, SimulatorMotionData> datasDic);
        #endregion

        #region 事件
        /// <summary>
        /// 变量变更事件
        /// </summary>
        public event UpdateVariableValueChanged OnUpdateVariableValueChanged;
        #endregion
        /// <summary>
        /// 初始化
        /// </summary>
        public void Initialize()
        {
            _periodicJob = new PeriodicJob(TIMER_INTERVAL, OnTimer, "Motor Simulator Timer", true);
            Init();
        }
        /// <summary>
        /// 初始化数据
        /// </summary>
        private void Init()
        {
            //加载对应配置文件 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;
                        }
                    }
                }
            }
            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;
                        _motorNameMinStepDic[item.Name] = (int)((value < 100) ? 100 : value);                        
                    }
                }
            }
            catch
            {
                LOG.WriteLog(eEvent.ERR_AXIS, "axisProvider", "Load AxisProviderCfg.xml failed");
            }
        }
        /// <summary>
        /// 定时器执行
        /// </summary>
        /// <returns></returns>
        private bool OnTimer()
        {
            //电机运动模型
            foreach(var motorItem in _motorNameDataDic)
            {
                //对应电机进行模拟
                MotorMotionSimulator(motorItem);
                //实时更新电机数据
                UpdateVariableValue(_motorNameDataDic);
            }
            
            return true;
        }
        /// <summary>
        /// 通知Galil模块数据变化
        /// </summary>
        /// <param name="data"></param>
        private void UpdateVariableValue(Dictionary<string, SimulatorMotionData> datasDic)
        {
            if (OnUpdateVariableValueChanged != null)
            {
                OnUpdateVariableValueChanged(datasDic);
            }
        }
        /// <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;
        }

    }
}