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;
}
}
}