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 FI_HOMING_SIGNAL = "FIHomingSignal";
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(KeyValuePair<string, SimulatorMotionData> datasDic);
public delegate void UpdateVariableValueInputDatasChanged(string module, string VariableName, bool value);
public delegate void UpdateVariableValueWagoDatasChanged(string sourceName,string targetName, bool value, bool invert);
#endregion
#region 事件
/// <summary>
/// MotionDatas变更事件
/// </summary>
public event UpdateVariableValueMotionDatasChanged OnUpdateMotionDatasChanged;
/// <summary>
/// InputDatas变更事件
/// </summary>
public event UpdateVariableValueInputDatasChanged OnUpdateInputDatasChanged;
/// <summary>
/// WagoDatas变更事件
/// </summary>
public event UpdateVariableValueWagoDatasChanged OnUpdateWagoDatasChanged;
#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[item.Name] = new SimulatorMotionData();
_motorNameDataDic[item.Name].FwdSoftLimit = item.ForwardSoftwareLimit;
_motorNameDataDic[item.Name].RevSoftLimit = item.ReverseSoftwareLimit;
_motorNameDataDic[item.Name].NegativeTorqueLimit = item.NegativeTorqueLimit;
_motorNameDataDic[item.Name].PositiveTorqueLimit = item.PositiveTorqueLimit;
_motorNameDataDic[item.Name].SwitchSignal = true;
_motorNameDataDic[item.Name].ModuleName = config.Module;
_motorNameDataDic[item.Name].ReverseLimit = true;
_motorNameDataDic[item.Name].ForwardLimit = 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;
_axisDataDic[item.Name] = item;
_motorNameMinStepDic[item.Name] = (int)((value < 200) ? 200 : 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;
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(motorItem);
//更新InputDatas
UpdateInputDatas(motorItem);
}
UpdateDatasByMutiAxisDatas();
return true;
}
/// <summary>
/// 通知Galil模块motion数据变化
/// </summary>
/// <param name="data"></param>
private void UpdateMotionDatas(KeyValuePair<string, SimulatorMotionData> dataPair)
{
if (OnUpdateMotionDatasChanged != null)
{
OnUpdateMotionDatasChanged(dataPair);
}
}
/// <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);
}
}
private void UpdateDatasByMutiAxisDatas()
{
if (OnUpdateWagoDatasChanged != null && CheckMutiMotionData(out string variableName, out bool value))
{
OnUpdateWagoDatasChanged("",variableName, value, false);
}
}
/// <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 FI_HOMING_SIGNAL:
_motorNameDataDic[axisName].FIHomingSignal = 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 || motionData.FIHomingSignal)
{
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.ForwardLimit = !fwdLimit;
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.ReverseLimit = !revLimit;
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>
/// <returns></returns>
private bool CheckMutiMotionData(out string variableName, out bool value)
{
bool result = false;
variableName = "";
value = false;
//Loader WS Present Sensor
if (CheckAtStation("Transporter2.Elevator", _motorNameDataDic["Transporter2.Elevator"], "Loader")
&& CheckAtStation("Transporter2.Gantry", _motorNameDataDic["Transporter2.Gantry"], "Loader"))
{
value = !_otherModuleDataDic[FESTO_DATABUFFER_TRANSPORT2_LOCK] ? true : false;
variableName = "r_Cathode_Present";
result = true;
}
return result;
}
/// <summary>
/// 更新其他模块数据
/// </summary>
/// <param name="name"></param>
/// <param name="value"></param>
private void UpdataDataCausedByOtherModule(string sourceName,string name, bool value, bool revert)
{
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;
}
}
}