meswork/01-Siyuan-MesProject
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

init: 导入项目到 meswork Gitea

Browse Source
This commit is contained in:
XingCheng3
2026-05-29 10:07:05 +08:00
commit ad9a4106fb
3582 changed files with 460312 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

553
DataMirrorTools_Simulator/Foundation/Kinematics/Kinematics.Sensor.cs Normal file
View File

@@ -0,0 +1,553 @@
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
}