253 lines
9.5 KiB
C#
253 lines
9.5 KiB
C#
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;
|
|
}
|
|
}
|