554 lines
18 KiB
C#
554 lines
18 KiB
C#
using System;
|
||
using System.Collections;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
namespace KinematicsBase
|
||
{
|
||
#region BaseClass
|
||
#region Data Define
|
||
/// <summary>
|
||
/// 传感器类型
|
||
/// </summary>
|
||
[Serializable]
|
||
public enum SensorType
|
||
{
|
||
/// <summary>
|
||
/// 姿态传感器
|
||
/// </summary>
|
||
Pose,
|
||
/// <summary>
|
||
/// 关节距离传感器
|
||
/// </summary>
|
||
Distance,
|
||
/// <summary>
|
||
/// 关节传感器
|
||
/// 判断关节位置
|
||
/// </summary>
|
||
Joint,
|
||
/// <summary>
|
||
/// 接近传感器
|
||
/// </summary>
|
||
Near,
|
||
/// <summary>
|
||
/// 光电传感器
|
||
/// </summary>
|
||
PhotoElectric,
|
||
/// <summary>
|
||
/// 属性传感器
|
||
/// </summary>
|
||
Attribution,
|
||
/// <summary>
|
||
/// Joint的值变化 ture
|
||
/// 不变化 false
|
||
/// </summary>
|
||
IsRun
|
||
|
||
}
|
||
/// <summary>
|
||
/// 信号触发方式
|
||
/// </summary>
|
||
/// [Serializable]
|
||
public enum TrigerType
|
||
{
|
||
Pulse,
|
||
Limit,
|
||
StepBegin,
|
||
StepEnd
|
||
}
|
||
[Serializable]
|
||
|
||
public enum SensorDataType
|
||
{
|
||
|
||
INT,
|
||
|
||
DINT,
|
||
|
||
FLOAT,
|
||
|
||
DOUBLE
|
||
}
|
||
#endregion
|
||
/// <summary>
|
||
/// 姿态传感器
|
||
/// </summary>
|
||
[Serializable]
|
||
public class Sensor
|
||
{
|
||
///// <summary>
|
||
///// 向外部发出传感器信号
|
||
///// </summary>
|
||
//public static event Action<string, object> OnDataChange = null;
|
||
|
||
/// <summary>
|
||
/// string ModeId, string PoseId, string JointId, string Id, object Value
|
||
/// </summary>
|
||
public static event Action<string, string, string, string, object> OnDataChange;
|
||
//public static event Action<string, string, string, string, bool> OnDataChangeBool;
|
||
//public static event Action<string, string, string, string, int> OnDataChangeInt;
|
||
//public static event Action<string, string, string, string, double> OnDataChangeFloat;
|
||
|
||
public string Id { get; set; }
|
||
|
||
public string Name { get; set; }
|
||
public string ModelId { get; set; }
|
||
public bool I = false;
|
||
public SensorType SensorType { get; set; }
|
||
public TrigerType TrigerType { get; set; }
|
||
public string PoseId { get; set; } = "";
|
||
/// <summary>
|
||
/// PoseId =0和姿态距离传感器用
|
||
/// </summary>
|
||
public string JointId { get; set; } = "";
|
||
public UnitType UnitType { get; set; }
|
||
public SensorDataType SensorDataType { get; set; }
|
||
|
||
/// <summary>
|
||
/// 传感器是否启用
|
||
/// </summary>
|
||
public bool IsEnable { get; set; }
|
||
///// <summary>
|
||
///// PLC 变量代码
|
||
///// </summary>
|
||
//public string SignalViewer_Id;
|
||
|
||
/// <summary>
|
||
/// 姿态传感器 光电传感器
|
||
/// 触发后返回值 0，1
|
||
/// 或伺服值
|
||
/// </summary>
|
||
public object Tagvalue { get; set; }
|
||
/// <summary>
|
||
/// 触发值测试误差值上限
|
||
/// </summary>
|
||
public double LimitUpValue { get; set; } = 2f;
|
||
/// <summary>
|
||
/// 触发值测试误差值下限
|
||
/// </summary>
|
||
public double LimitDownValue { get; set; } = -2f;
|
||
|
||
/// <summary>
|
||
/// 光电传感器 检测长度用
|
||
/// </summary>
|
||
public double Length { get; set; }
|
||
public double ValueFactor { get; set; } = 1;
|
||
|
||
public Sensor()
|
||
{
|
||
this.Id = Guid.NewGuid().ToString("D").ToUpper();
|
||
|
||
}
|
||
public void Setup()
|
||
{
|
||
switch (SensorType)
|
||
{
|
||
case SensorType.Pose:
|
||
SetupPose();
|
||
break;
|
||
case SensorType.Distance:
|
||
SetupDistance();
|
||
break;
|
||
case SensorType.Joint:
|
||
SetupJoint();
|
||
break;
|
||
}
|
||
}
|
||
private void SetupJoint()
|
||
{
|
||
bool isTriger = false;
|
||
//Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == ModelId);
|
||
var k = PP.ModelCodeFindModelAttribute(ModelId).KinematicsData;
|
||
if (k == null) return;
|
||
var val = k.JointList.FirstOrDefault(t => t.Id == JointId).ValueCurrent;
|
||
var jValue = k.PoseList.FirstOrDefault(t => t.Id == PoseId).PoseInfo[JointId];
|
||
|
||
if ((val >= jValue + LimitDownValue) && (val <= jValue + LimitDownValue))
|
||
{
|
||
isTriger = true;
|
||
}
|
||
else
|
||
{
|
||
isTriger = false;
|
||
}
|
||
I = isTriger;
|
||
DataChange(ModelId, PoseId, JointId, Id, I);
|
||
}
|
||
|
||
private void SetupDistance()
|
||
{
|
||
// Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == ModelId);
|
||
var k = PP.ModelCodeFindModelAttribute(ModelId).KinematicsData;
|
||
if (k == null) return;
|
||
double val = k.JointList.FirstOrDefault(t => t.Id == JointId).ValueCurrent;
|
||
|
||
object value;
|
||
|
||
switch (SensorDataType)
|
||
{
|
||
case SensorDataType.INT:
|
||
value = Convert.ToInt32(val);
|
||
DataChange(ModelId, PoseId, JointId, Id, value);
|
||
|
||
break;
|
||
case SensorDataType.DINT:
|
||
value = (long)val;
|
||
DataChange(ModelId, PoseId, JointId, Id, value);
|
||
break;
|
||
case SensorDataType.FLOAT:
|
||
value = (double)val;
|
||
DataChange(ModelId, PoseId, JointId, Id, value);
|
||
break;
|
||
case SensorDataType.DOUBLE:
|
||
value = (double)val;
|
||
DataChange(ModelId, PoseId, JointId, Id, value);
|
||
break;
|
||
}
|
||
}
|
||
private void SetupPose()
|
||
{
|
||
//double val = (double)vale;
|
||
bool isTriger = false;
|
||
bool isTrigerJoint;
|
||
int index = 0;
|
||
//Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == ModelId);
|
||
var k = PP.ModelCodeFindModelAttribute(ModelId).KinematicsData;
|
||
if (k == null) return;
|
||
|
||
//监控POSE的全部姿态
|
||
var pose = k.PoseList.FirstOrDefault(t => t.Id == PoseId);
|
||
if (pose == null) return;
|
||
foreach (var item in pose.PoseInfo)
|
||
{
|
||
double currentValue = k.JointList.FirstOrDefault(t => t.Id == item.Key).ValueCurrent;
|
||
if ((currentValue >= item.Value + LimitDownValue) && (currentValue <= item.Value + LimitUpValue))
|
||
{
|
||
isTrigerJoint = true;
|
||
}
|
||
else
|
||
{
|
||
isTrigerJoint = false;
|
||
}
|
||
if (!isTrigerJoint)
|
||
{
|
||
isTriger = false;
|
||
break;
|
||
}
|
||
else
|
||
{
|
||
if (index == 0)
|
||
{
|
||
isTriger = isTrigerJoint;
|
||
}
|
||
else
|
||
{
|
||
isTriger = isTriger && isTrigerJoint;
|
||
}
|
||
if (isTriger == false)
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
index++;
|
||
}
|
||
I = isTriger;
|
||
DataChange(ModelId, PoseId, JointId, Id, I);
|
||
}
|
||
public Sensor(SensorType SensorType)
|
||
{
|
||
this.Id = Guid.NewGuid().ToString("D").ToUpper();
|
||
Tagvalue = 0;
|
||
switch (SensorType)
|
||
{
|
||
case SensorType.IsRun:
|
||
Joint.OnDataChangeJointIsRun -= Joint_OnDataChangeJointIsRun;
|
||
Joint.OnDataChangeJointIsRun += Joint_OnDataChangeJointIsRun;
|
||
break;
|
||
default:
|
||
Joint.OnDataChange -= Joint_OnDataChange;
|
||
Joint.OnDataChange += Joint_OnDataChange;
|
||
break;
|
||
}
|
||
}
|
||
public void Joint_OnDataChangeJointIsRun(string modelId, string jointId, bool val)
|
||
{
|
||
if (this.ModelId == modelId && this.JointId == jointId)
|
||
{
|
||
I = val;
|
||
DataChange(ModelId, PoseId, JointId, Id, I);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
public string CreateSensorPose(string modelId, string poseId)
|
||
{
|
||
this.Id = Guid.NewGuid().ToString("D").ToUpper();
|
||
Joint.OnDataChange -= Joint_OnDataChange;
|
||
Joint.OnDataChange += Joint_OnDataChange;
|
||
Tagvalue = 0;
|
||
|
||
this.IsEnable = true;
|
||
this.PoseId = poseId;
|
||
var kTemp = Kinematics.GetKinematicsByModelId(modelId);
|
||
{
|
||
this.Name = kTemp.ModelName + "_PS_Logical_1";
|
||
this.SensorType = SensorType.Pose;
|
||
}
|
||
this.ModelId = modelId;
|
||
this.LimitUpValue = 2;
|
||
this.LimitDownValue = -2;
|
||
kTemp.SensorList.Add(this);
|
||
// Kinematics.SaveToModel(kTemp);
|
||
if (GlobalEventInterface.E_Transmit_SaveToModel != null)
|
||
{
|
||
GlobalEventInterface.E_Transmit_SaveToModel(kTemp);
|
||
}
|
||
|
||
return Id;
|
||
}
|
||
public string CreateSensorJoint(string modelId, string poseId, string jointId)
|
||
{
|
||
this.Id = Guid.NewGuid().ToString("D").ToUpper();
|
||
Joint.OnDataChange -= Joint_OnDataChange;
|
||
Joint.OnDataChange += Joint_OnDataChange;
|
||
Tagvalue = 0;
|
||
this.IsEnable = true;
|
||
this.PoseId = poseId;
|
||
this.JointId = jointId;
|
||
var kTemp = Kinematics.GetKinematicsByModelId(modelId);
|
||
|
||
this.Name = kTemp.ModelName + "_PJ_Logical_1";
|
||
this.SensorType = SensorType.Joint;
|
||
this.ModelId = modelId;
|
||
this.LimitUpValue = 2;
|
||
this.LimitDownValue = -2;
|
||
kTemp.SensorList.Add(this);
|
||
// Kinematics.SaveToModel(kTemp);
|
||
if (GlobalEventInterface.E_Transmit_SaveToModel != null)
|
||
{
|
||
GlobalEventInterface.E_Transmit_SaveToModel(kTemp);
|
||
}
|
||
return Id;
|
||
}
|
||
public string CreateSensorJointIsRun(string modelId, string jointId)
|
||
{
|
||
this.Id = Guid.NewGuid().ToString("D").ToUpper();
|
||
Joint.OnDataChangeJointIsRun -= Joint_OnDataChangeJointIsRun;
|
||
Joint.OnDataChangeJointIsRun += Joint_OnDataChangeJointIsRun;
|
||
|
||
Tagvalue = 0;
|
||
|
||
this.IsEnable = true;
|
||
this.JointId = jointId;
|
||
var kTemp = Kinematics.GetKinematicsByModelId(modelId);
|
||
|
||
this.Name = kTemp.ModelName + "_Run_Logical_1";
|
||
this.SensorType = SensorType.IsRun;
|
||
this.ModelId = modelId;
|
||
kTemp.SensorList.Add(this);
|
||
// Kinematics.SaveToModel(kTemp);
|
||
if (GlobalEventInterface.E_Transmit_SaveToModel != null)
|
||
{
|
||
GlobalEventInterface.E_Transmit_SaveToModel(kTemp);
|
||
}
|
||
return Id;
|
||
}
|
||
public string CreateSensorPoseDistance(string modelId, string jointId, SensorDataType sensorDataType)
|
||
{
|
||
this.Id = Guid.NewGuid().ToString("D").ToUpper();
|
||
Joint.OnDataChange -= Joint_OnDataChange;
|
||
Joint.OnDataChange += Joint_OnDataChange;
|
||
Tagvalue = 0;
|
||
|
||
var kTemp = Kinematics.GetKinematicsByModelId(modelId);
|
||
|
||
this.IsEnable = true;
|
||
this.JointId = jointId;
|
||
this.SensorType = SensorType.Distance;
|
||
this.Name = kTemp.ModelName + "_JD_Logical_1";
|
||
this.ModelId = modelId;
|
||
this.SensorDataType = sensorDataType;
|
||
this.LimitUpValue = 2;
|
||
this.LimitDownValue = -2;
|
||
kTemp.SensorList.Add(this);
|
||
// Kinematics.SaveToModel(kTemp);
|
||
if (GlobalEventInterface.E_Transmit_SaveToModel != null)
|
||
{
|
||
GlobalEventInterface.E_Transmit_SaveToModel(kTemp);
|
||
}
|
||
return Id;
|
||
}
|
||
|
||
public static void DataChange(string ModeId, string PoseId, string JointId, string Id, object Value)
|
||
{
|
||
if (OnDataChange != null)
|
||
{
|
||
OnDataChange.Invoke(ModeId, PoseId, JointId, Id, Value);
|
||
}
|
||
}
|
||
|
||
|
||
public void Joint_OnDataChange(string modeId, string jointID, object val)
|
||
{
|
||
//if (this.Id == null) return;
|
||
if (this.ModelId == modeId)
|
||
{
|
||
Triger(jointID, val);
|
||
|
||
}
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 根据Joint姿态的值，触发传感器信号
|
||
/// </summary>
|
||
/// <param name="val"></param>
|
||
public void Triger(string jointID, object val)
|
||
{
|
||
switch (SensorType)
|
||
{
|
||
case SensorType.Pose:
|
||
TrigerPose(val);
|
||
break;
|
||
case SensorType.Distance:
|
||
TrigerDistance(jointID, val);
|
||
break;
|
||
case SensorType.Joint:
|
||
TrigerJoint(jointID, val);
|
||
break;
|
||
case SensorType.PhotoElectric:
|
||
break;
|
||
case SensorType.Near:
|
||
break;
|
||
case SensorType.Attribution:
|
||
break;
|
||
}
|
||
}
|
||
/// <summary>
|
||
/// 回写关节当前值（距离）
|
||
/// 可以订阅SensorDistanceDataChange事件
|
||
/// </summary>
|
||
/// <param name="val"></param>
|
||
public void TrigerDistance(string jointID, object val)
|
||
{
|
||
if (this.JointId != jointID) return;
|
||
object value;
|
||
|
||
switch (SensorDataType)
|
||
{
|
||
case SensorDataType.INT:
|
||
value = Convert.ToInt32(val);
|
||
DataChange(ModelId, PoseId, JointId, Id, value);
|
||
|
||
break;
|
||
case SensorDataType.DINT:
|
||
value = (long)val;
|
||
DataChange(ModelId, PoseId, JointId, Id, value);
|
||
break;
|
||
case SensorDataType.FLOAT:
|
||
value = (double)val;
|
||
DataChange(ModelId, PoseId, JointId, Id, value);
|
||
break;
|
||
case SensorDataType.DOUBLE:
|
||
value = (double)val;
|
||
DataChange(ModelId, PoseId, JointId, Id, value);
|
||
break;
|
||
}
|
||
}
|
||
/// <summary>
|
||
/// 判断姿态位置
|
||
/// </summary>
|
||
/// <param name="val"></param>
|
||
public void TrigerPose(object vale)
|
||
{
|
||
double val = (double)vale;
|
||
bool isTriger = false;
|
||
bool isTrigerJoint;
|
||
int index = 0;
|
||
//Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == ModelId);
|
||
var k = PP.ModelCodeFindModelAttribute(ModelId).KinematicsData;
|
||
if (k == null) return;
|
||
|
||
//监控POSE的全部姿态
|
||
var pose = k.PoseList.FirstOrDefault(t => t.Id == PoseId);
|
||
if (pose == null) return;
|
||
foreach (var item in pose.PoseInfo)
|
||
{
|
||
double currentValue = k.JointList.FirstOrDefault(t => t.Id == item.Key).ValueCurrent;
|
||
if ((currentValue >= item.Value + LimitDownValue) && (currentValue <= item.Value + LimitUpValue))
|
||
{
|
||
isTrigerJoint = true;
|
||
}
|
||
else
|
||
{
|
||
isTrigerJoint = false;
|
||
}
|
||
if (!isTrigerJoint)
|
||
{
|
||
isTriger = false;
|
||
break;
|
||
}
|
||
else
|
||
{
|
||
if (index == 0)
|
||
{
|
||
isTriger = isTrigerJoint;
|
||
}
|
||
else
|
||
{
|
||
isTriger = isTriger && isTrigerJoint;
|
||
}
|
||
if (isTriger == false)
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
index++;
|
||
}
|
||
if (I != isTriger)
|
||
{
|
||
I = isTriger;
|
||
DataChange(ModelId, PoseId, JointId, Id, I);
|
||
//DataChangeBool(ModelId, PoseId, JointId, Id, I);
|
||
}
|
||
}
|
||
/// <summary>
|
||
/// 关节传感器
|
||
/// 判断关节位置
|
||
/// </summary>
|
||
/// <param name="jointID"></param>
|
||
/// <param name="val"></param>
|
||
public void TrigerJoint(string jointID, object vale)
|
||
{
|
||
if (this.JointId != jointID) return;
|
||
|
||
double val = (double)vale;
|
||
bool isTriger = false;
|
||
// Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == ModelId);
|
||
var k = PP.ModelCodeFindModelAttribute(ModelId).KinematicsData;
|
||
if (k == null) return;
|
||
var jValue = k.PoseList.FirstOrDefault(t => t.Id == PoseId).PoseInfo[jointID];
|
||
|
||
if ((val >= jValue + LimitDownValue) && (val <= jValue + LimitDownValue))
|
||
{
|
||
isTriger = true;
|
||
}
|
||
else
|
||
{
|
||
isTriger = false;
|
||
}
|
||
if (I != isTriger)
|
||
{
|
||
I = isTriger;
|
||
DataChange(ModelId, PoseId, JointId, Id, I);
|
||
//DataChangeBool(ModelId, PoseId, JointId, Id, I);
|
||
}
|
||
}
|
||
|
||
}
|
||
//
|
||
#endregion
|
||
}
|
||
|
||
|