DungTT/RobotNet
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit -push

Browse Source
This commit is contained in:
dungtt
2025-10-15 15:15:53 +07:00
parent 674ae395be
commit a9577c5756
885 changed files with 74595 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

252
RobotNet.RobotManager/Services/Simulation/Algorithm/FuzzyLogic.cs Normal file
View File

@@ -0,0 +1,252 @@
namespace RobotNet.RobotManager.Services.Simulation.Algorithm;
public class FuzzyLogic
{
private double Gain_P = 0.5;
private double Gain_I = 0.01;
private double DiscreteTimeIntegrator_DSTATE;
public FuzzyLogic WithGainP(double gainP)
{
Gain_P = gainP;
return this;
}
public FuzzyLogic WithGainI(double gainI)
{
Gain_I = gainI;
return this;
}
private static double Fuzzy_trapmf(double x, double[] parame)
{
double b_y1;
double y2;
b_y1 = 0.0;
y2 = 0.0;
if (x >= parame[1])
{
b_y1 = 1.0;
}
if (x < parame[0])
{
b_y1 = 0.0;
}
if (parame[0] <= x && x < parame[1] && parame[0] != parame[1])
{
b_y1 = 1.0 / (parame[1] - parame[0]) * (x - parame[0]);
}
if (x <= parame[2])
{
y2 = 1.0;
}
if (x > parame[3])
{
y2 = 0.0;
}
if (parame[2] < x && x <= parame[3] && parame[2] != parame[3])
{
y2 = 1.0 / (parame[3] - parame[2]) * (parame[3] - x);
}
return b_y1 < y2 ? b_y1 : y2;
}
private static double Fuzzy_trimf(double x, double[] parame)
{
double y;
y = 0.0;
if (parame[0] != parame[1] && parame[0] < x && x < parame[1])
{
y = 1.0 / (parame[1] - parame[0]) * (x - parame[0]);
}
if (parame[1] != parame[2] && parame[1] < x && x < parame[2])
{
y = 1.0 / (parame[2] - parame[1]) * (parame[2] - x);
}
if (x == parame[1])
{
y = 1.0;
}
return y;
}
public (double wl, double wr) Fuzzy_step(double v, double w, double TimeSample)
{
(double wl, double wr) result = new();
double[] inputMFCache = new double[10];
double[] outputMFCache = new double[5];
double[] outputMFCache_0 = new double[5];
double[] tmp = new double[3];
double aggregatedOutputs;
double rtb_TmpSignalConversionAtSFun_0;
double rtb_antecedentOutputs_e;
double sumAntecedentOutputs;
int ruleID;
double[] f = [-1.0E+10, -1.0E+10, -1.0, -0.5];
double[] e = [0.5, 1.0, 1.0E+10, 1.0E+10];
double[] d = [0.75, 1.0, 1.0E+9, 1.0E+9];
double[] c = [-1.0E+9, -1.0E+9, 0.0, 0.25];
byte[] b = [ 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 4,
4, 4, 4, 4, 5, 5, 5, 5, 5, 1, 2, 3, 4, 5, 1, 2, 3,
4, 5, 1, 2, 3, 4, 5, 3, 4, 5, 1, 2, 1, 2, 3, 4, 5 ];
byte[] b_0 = [1, 1, 2, 1, 1, 2, 3, 5, 1, 4, 5, 5, 5, 5, 5, 2, 1, 1, 1, 1, 5, 5, 5, 5, 5];
byte[] b_1 = [ 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 4,
4, 4, 4, 4, 5, 5, 5, 5, 5, 1, 2, 3, 4, 5, 4, 1,
2, 3, 5, 3, 1, 2, 4, 5, 1, 2, 3, 4, 5, 1, 2, 4, 5, 3 ];
byte[] b_2 = [5, 5, 5, 5, 5, 1, 2, 3, 5, 4, 2, 1, 1, 1, 1, 5, 5, 5, 5, 5, 1, 1, 1, 1, 2];
double inputMFCache_tmp;
double inputMFCache_tmp_0;
double inputMFCache_tmp_1;
double inputMFCache_tmp_2;
/* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic Controller1' */
/* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic Controller' */
/* SignalConversion generated from: '<S4>/ SFunction ' incorporates:
* Constant: '<Root>/w'
* DiscreteIntegrator: '<Root>/Discrete-Time Integrator'
* Gain: '<Root>/Gain1'
* MATLAB Function: '<S1>/Evaluate Rule Antecedents'
* MATLAB Function: '<S2>/Evaluate Rule Antecedents'
* SignalConversion generated from: '<S7>/ SFunction '
* Sum: '<Root>/Sum'
*/
DiscreteTimeIntegrator_DSTATE += Gain_I * w * TimeSample;
rtb_TmpSignalConversionAtSFun_0 = Gain_P * w + DiscreteTimeIntegrator_DSTATE;
/* End of Outputs for SubSystem: '<Root>/Fuzzy Logic Controller1' */
/* MATLAB Function: '<S1>/Evaluate Rule Antecedents' incorporates:
* Constant: '<Root>/v'
* MATLAB Function: '<S2>/Evaluate Rule Antecedents'
* SignalConversion generated from: '<S4>/ SFunction '
*/
sumAntecedentOutputs = 0.0;
/* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic Controller1' */
inputMFCache_tmp = Fuzzy_trapmf(rtb_TmpSignalConversionAtSFun_0, f);
/* End of Outputs for SubSystem: '<Root>/Fuzzy Logic Controller1' */
inputMFCache[0] = inputMFCache_tmp;
tmp[0] = -0.5;
tmp[1] = 0.0;
tmp[2] = 0.5;
inputMFCache[1] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
/* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic Controller1' */
inputMFCache_tmp_0 = Fuzzy_trapmf(rtb_TmpSignalConversionAtSFun_0, e);
/* End of Outputs for SubSystem: '<Root>/Fuzzy Logic Controller1' */
inputMFCache[2] = inputMFCache_tmp_0;
tmp[0] = -1.0;
tmp[1] = -0.5;
tmp[2] = 0.0;
inputMFCache[3] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
tmp[0] = 0.0;
tmp[1] = 0.5;
tmp[2] = 1.0;
inputMFCache[4] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
tmp[0] = 0.0;
tmp[1] = 0.25;
tmp[2] = 0.5;
inputMFCache[5] = Fuzzy_trimf(v, tmp);
tmp[0] = 0.25;
tmp[1] = 0.5;
tmp[2] = 0.75;
inputMFCache[6] = Fuzzy_trimf(v, tmp);
/* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic Controller1' */
inputMFCache_tmp_1 = Fuzzy_trapmf(v, d);
/* End of Outputs for SubSystem: '<Root>/Fuzzy Logic Controller1' */
inputMFCache[7] = inputMFCache_tmp_1;
/* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic Controller1' */
inputMFCache_tmp_2 = Fuzzy_trapmf(v, c);
/* End of Outputs for SubSystem: '<Root>/Fuzzy Logic Controller1' */
inputMFCache[8] = inputMFCache_tmp_2;
tmp[0] = 0.5;
tmp[1] = 0.75;
tmp[2] = 1.0;
inputMFCache[9] = Fuzzy_trimf(v, tmp);
/* MATLAB Function: '<S1>/Evaluate Rule Consequents' */
aggregatedOutputs = 0.0;
outputMFCache[0] = 0.0;
outputMFCache[1] = 0.25;
outputMFCache[2] = 0.5;
outputMFCache[3] = 0.75;
outputMFCache[4] = 1.0;
for (ruleID = 0; ruleID < 25; ruleID++)
{
/* MATLAB Function: '<S1>/Evaluate Rule Antecedents' */
rtb_antecedentOutputs_e = inputMFCache[b[ruleID + 25] + 4] * inputMFCache[b[ruleID] - 1];
sumAntecedentOutputs += rtb_antecedentOutputs_e;
/* MATLAB Function: '<S1>/Evaluate Rule Consequents' */
aggregatedOutputs += outputMFCache[b_0[ruleID] - 1] * rtb_antecedentOutputs_e;
}
/* MATLAB Function: '<S1>/Defuzzify Outputs' incorporates:
* MATLAB Function: '<S1>/Evaluate Rule Antecedents'
* MATLAB Function: '<S1>/Evaluate Rule Consequents'
*/
if (sumAntecedentOutputs == 0.0)
{
result.wr = 0.5;
}
else
{
result.wr = 1.0 / sumAntecedentOutputs * aggregatedOutputs;
}
/* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic Controller1' */
/* MATLAB Function: '<S2>/Evaluate Rule Antecedents' incorporates:
* Constant: '<Root>/v'
* SignalConversion generated from: '<S7>/ SFunction '
*/
sumAntecedentOutputs = 0.0;
inputMFCache[0] = inputMFCache_tmp;
tmp[0] = -0.5;
tmp[1] = 0.0;
tmp[2] = 0.5;
inputMFCache[1] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
inputMFCache[2] = inputMFCache_tmp_0;
tmp[0] = -1.0;
tmp[1] = -0.5;
tmp[2] = 0.0;
inputMFCache[3] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
tmp[0] = 0.0;
tmp[1] = 0.5;
tmp[2] = 1.0;
inputMFCache[4] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
tmp[0] = 0.0;
tmp[1] = 0.25;
tmp[2] = 0.5;
inputMFCache[5] = Fuzzy_trimf(v, tmp);
tmp[0] = 0.25;
tmp[1] = 0.5;
tmp[2] = 0.75;
inputMFCache[6] = Fuzzy_trimf(v, tmp);
inputMFCache[7] = inputMFCache_tmp_1;
inputMFCache[8] = inputMFCache_tmp_2;
tmp[0] = 0.5;
tmp[1] = 0.75;
tmp[2] = 1.0;
inputMFCache[9] = Fuzzy_trimf(v, tmp);
/* MATLAB Function: '<S2>/Evaluate Rule Consequents' */
aggregatedOutputs = 0.0;
outputMFCache_0[0] = 0.0;
outputMFCache_0[1] = 0.25;
outputMFCache_0[2] = 0.5;
outputMFCache_0[3] = 0.75;
outputMFCache_0[4] = 1.0;
for (ruleID = 0; ruleID < 25; ruleID++)
{
/* MATLAB Function: '<S2>/Evaluate Rule Antecedents' */
rtb_antecedentOutputs_e = inputMFCache[b_1[ruleID + 25] + 4] * inputMFCache[b_1[ruleID] - 1];
sumAntecedentOutputs += rtb_antecedentOutputs_e;
/* MATLAB Function: '<S2>/Evaluate Rule Consequents' */
aggregatedOutputs += outputMFCache_0[b_2[ruleID] - 1] *
rtb_antecedentOutputs_e;
}
/* MATLAB Function: '<S2>/Defuzzify Outputs' incorporates:
* MATLAB Function: '<S2>/Evaluate Rule Antecedents'
* MATLAB Function: '<S2>/Evaluate Rule Consequents'
*/
if (sumAntecedentOutputs == 0.0)
{
result.wl = 0.5;
}
else
{
result.wl = 1.0 / sumAntecedentOutputs * aggregatedOutputs;
}
return result;
}
}

8
RobotNet.RobotManager/Services/Simulation/Algorithm/MathExtension.cs Normal file
View File

@@ -0,0 +1,8 @@
namespace RobotNet.RobotManager.Services.Simulation.Algorithm;
public class MathExtension
{
public static double ToRad => Math.PI / 180;
public static double ToDegree => 180 / Math.PI;
public static double CheckLimit(double value, double Max, double Min) => value < Max ? value > Min ? value : Min : Max;
}

42
RobotNet.RobotManager/Services/Simulation/Algorithm/PID.cs Normal file
View File

@@ -0,0 +1,42 @@
namespace RobotNet.RobotManager.Services.Simulation.Algorithm;
public class PID
{
private double Kp = 0.3;
private double Ki = 0.0001;
private double Kd = 0.01;
private double Pre_Error;
private double Pre_Pre_Error;
private double Pre_Out;
public PID WithKp(double kp)
{
Kp = kp;
return this;
}
public PID WithKi(double ki)
{
Ki = ki;
return this;
}
public PID WithKd(double kd)
{
Kd = kd;
return this;
}
public double PID_step(double Error, double LimitMax, double LimitMin, double TimeSample)
{
double P_part = Kp * (Error - Pre_Error);
double I_part = 0.5 * Ki * TimeSample * (Error + Pre_Error);
double D_part = Kd / TimeSample * (Error - 2 * Pre_Error + Pre_Pre_Error);
double Out = Pre_Out + P_part + I_part + D_part;
Pre_Pre_Error = Pre_Error;
Pre_Error = Error;
Pre_Out = Out;
Out = MathExtension.CheckLimit(Out, LimitMax, LimitMin);
return Out;
}
}

159
RobotNet.RobotManager/Services/Simulation/Algorithm/PurePursuit.cs Normal file
View File

@@ -0,0 +1,159 @@
using RobotNet.RobotManager.Services.Simulation.Models;
using RobotNet.RobotShares.Enums;
namespace RobotNet.RobotManager.Services.Simulation.Algorithm;
public class PurePursuit
{
private double MaxAngularVelocity = 1.5;
private double LookaheadDistance = 0.5;
private List<NavigationNode> Waypoints_Value = [];
public int OnNodeIndex = 0;
public int GoalIndex = 0;
public NavigationNode? Goal;
public PurePursuit WithLookheadDistance(double distance)
{
LookaheadDistance = distance;
return this;
}
public PurePursuit WithMaxAngularVelocity(double vel)
{
MaxAngularVelocity = vel;
return this;
}
public PurePursuit WithPath(NavigationNode[] path)
{
Waypoints_Value = [.. path];
return this;
}
public void UpdatePath(NavigationNode[] path)
{
Waypoints_Value = [.. path];
}
public void UpdateGoal(NavigationNode goal)
{
Goal = goal;
GoalIndex = Waypoints_Value.IndexOf(Goal);
}
public (NavigationNode node, int index) GetOnNode(double x, double y)
{
double minDistance = double.MaxValue;
NavigationNode onNode = Waypoints_Value[0];
int index = 0;
for (int i = 1; i < Waypoints_Value.Count; i++)
{
var distance = Math.Sqrt(Math.Pow(x - Waypoints_Value[i].X, 2) + Math.Pow(y - Waypoints_Value[i].Y, 2));
if (distance < minDistance)
{
onNode = Waypoints_Value[i];
minDistance = distance;
index = i;
}
}
return (onNode, index);
}
private (NavigationNode? node, int index) OnNode(double x, double y)
{
if (Waypoints_Value is null || Waypoints_Value.Count == 0) return (null, 0);
double minDistance = double.MaxValue;
int index = 0;
NavigationNode? onNode = null;
if(Goal is null) return (onNode, index);
for (int i = OnNodeIndex; i < Waypoints_Value.IndexOf(Goal); i ++)
{
var distance = Math.Sqrt(Math.Pow(x - Waypoints_Value[i].X, 2) + Math.Pow(y - Waypoints_Value[i].Y, 2));
if (distance < minDistance)
{
onNode = Waypoints_Value[i];
minDistance = distance;
index = i;
}
}
return (onNode, index);
}
public double PurePursuit_step(double X_Ref, double Y_Ref, double Angle_Ref)
{
if (Waypoints_Value is null || Waypoints_Value.Count < 2) return 0;
NavigationNode? lookaheadStartPt = null;
var (onNode, index) = OnNode(X_Ref, Y_Ref);
if (onNode is null || Goal is null) return 0;
OnNodeIndex = index;
double lookDistance = 0;
for (int i = OnNodeIndex + 1; i < Waypoints_Value.IndexOf(Goal); i++)
{
lookDistance += Math.Sqrt(Math.Pow(Waypoints_Value[i - 1].X - Waypoints_Value[i].X, 2) + Math.Pow(Waypoints_Value[i - 1].Y - Waypoints_Value[i].Y, 2));
if (lookDistance >= LookaheadDistance || Waypoints_Value[i].Direction != onNode.Direction)
{
lookaheadStartPt = Waypoints_Value[i];
break;
}
}
lookaheadStartPt ??= Goal;
if (onNode.Direction == RobotDirection.BACKWARD)
{
if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2;
else if (Angle_Ref < -Math.PI) Angle_Ref += Math.PI * 2;
Angle_Ref += Math.PI;
if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2;
}
var distance = Math.Atan2(lookaheadStartPt.Y - Y_Ref, lookaheadStartPt.X - X_Ref) - Angle_Ref;
if (Math.Abs(distance) > Math.PI)
{
double minDistance;
if (distance + Math.PI == 0.0) minDistance = 0.0;
else
{
double data = (distance + Math.PI) / (2 * Math.PI);
if (data < 0) data = Math.Round(data + 0.5);
else data = Math.Round(data - 0.5);
minDistance = distance + Math.PI - data * (2 * Math.PI);
double checker = 0;
if (minDistance != 0.0)
{
checker = Math.Abs((distance + Math.PI) / (2 * Math.PI));
}
if (!(Math.Abs(checker - Math.Floor(checker + 0.5)) > 2.2204460492503131E-16 * checker))
{
minDistance = 0.0;
}
else if (distance + Math.PI < 0.0)
{
minDistance += Math.PI * 2;
}
}
if (minDistance == 0.0 && distance + Math.PI > 0.0)
{
minDistance = Math.PI * 2;
}
distance = minDistance - Math.PI;
}
var AngularVelocity = 2.0 * 0.5 * Math.Sin(distance) / LookaheadDistance;
if (Math.Abs(AngularVelocity) > MaxAngularVelocity)
{
if (AngularVelocity < 0.0)
{
AngularVelocity = -1.0;
}
else if (AngularVelocity > 0.0)
{
AngularVelocity = 1.0;
}
else if (AngularVelocity == 0.0)
{
AngularVelocity = 0.0;
}
AngularVelocity *= MaxAngularVelocity;
}
return AngularVelocity;
}
}