public static double GetResultantSpeed(int leftMotorSpeed, int rightMotorSpeed) //funkcja zwracająca prędkość wypadkową pojazdu
{
leftMotorSpeed /= 10;
rightMotorSpeed /= 10;
#region definicje zmiennych
double leftMotorSpeedRear = ElementaryFunctions.SpeedRear(leftMotorSpeed);
double leftMotorSpeedSlow = ElementaryFunctions.SpeedSlow(leftMotorSpeed);
double leftMotorSpeedMedium = ElementaryFunctions.SpeedMedium(leftMotorSpeed);
double leftMotorSpeedFast = ElementaryFunctions.SpeedFast(leftMotorSpeed);
double rightMotorSpeedRear = ElementaryFunctions.SpeedRear(rightMotorSpeed);
double rightMotorSpeedSlow = ElementaryFunctions.SpeedSlow(rightMotorSpeed);
double rightMotorSpeedMedium = ElementaryFunctions.SpeedMedium(rightMotorSpeed);
double rightMotorSpeedFast = ElementaryFunctions.SpeedFast(rightMotorSpeed);
double[] tabSlow = new double[7];
double[] tabMedium = new double[5];
double[] tabFast = new double[3];
#endregion
//deklaracja stałych
const int slowParameter = 10;
const int mediumParameter = 50;
const int fastParameter = 90;
//obliczenie wszystkich wartosci dla predkosci wypadkowej wolno
tabSlow[0] = leftMotorSpeedRear * rightMotorSpeedSlow;
tabSlow[1] = leftMotorSpeedRear * rightMotorSpeedMedium;
tabSlow[2] = leftMotorSpeedRear * rightMotorSpeedFast;
tabSlow[3] = leftMotorSpeedSlow * rightMotorSpeedRear;
tabSlow[4] = leftMotorSpeedSlow * rightMotorSpeedSlow;
tabSlow[5] = leftMotorSpeedMedium * rightMotorSpeedRear;
tabSlow[6] = leftMotorSpeedFast * rightMotorSpeedRear;
//obliczenie wszystkich wartosci dla predkosci wypadkowej średnio
tabMedium[0] = leftMotorSpeedSlow * rightMotorSpeedMedium;
tabMedium[1] = leftMotorSpeedSlow * rightMotorSpeedFast;
tabMedium[2] = leftMotorSpeedMedium * rightMotorSpeedSlow;
tabMedium[3] = leftMotorSpeedMedium * rightMotorSpeedMedium;
tabMedium[4] = leftMotorSpeedFast * rightMotorSpeedSlow;
//obliczenie wszystkich wartosci dla predkosci wypadkowej szybko
tabFast[0] = leftMotorSpeedMedium * rightMotorSpeedFast;
tabFast[1] = leftMotorSpeedFast * rightMotorSpeedMedium;
tabFast[2] = leftMotorSpeedFast * rightMotorSpeedFast;
//wybranie wartości max dla reguł pogrupowanych względem S-M-F
double resultantSpeedSlow = Max(tabSlow);
double resultantSpeedMedium = Max(tabMedium);
double resultantSpeedFast = Max(tabFast);
//defuzzyfikacja metodą środka ciężkości
double result = ((resultantSpeedSlow * slowParameter) + (resultantSpeedMedium * mediumParameter) + (resultantSpeedFast * fastParameter))
/ (resultantSpeedSlow + resultantSpeedMedium + resultantSpeedFast);
return(result);
}
public static double HowTheRouteRuns(double speed, double reading)
{
const int hardLeft = -100;
const int easyLeft = -50;
const int straight = 0;
const int easyRight = 50;
const int hardRight = 100;
speed /= 10;
#region HardLeft
double hardLeftMembership;
double temp;
temp = ElementaryFunctions.ReadingHardLeft(reading) * ElementaryFunctions.SpeedFast(speed);
hardLeftMembership = temp;
temp = ElementaryFunctions.ReadingHardLeft(reading) * ElementaryFunctions.SpeedMedium(speed);
if (temp > hardLeftMembership)
{
hardLeftMembership = temp;
}
temp = ElementaryFunctions.ReadingEasyLeft(reading) * ElementaryFunctions.SpeedFast(speed);
if (temp > hardLeftMembership)
{
hardLeftMembership = temp;
}
temp = ElementaryFunctions.ReadingHardLeft(reading) * ElementaryFunctions.SpeedSlow(speed);
if (temp > hardLeftMembership)
{
hardLeftMembership = temp;
}
#endregion
#region EasyLeft
double easyLeftMembership;
temp = ElementaryFunctions.ReadingEasyLeft(reading) * ElementaryFunctions.SpeedMedium(speed);
easyLeftMembership = temp;
temp = ElementaryFunctions.ReadingEasyLeft(reading) * ElementaryFunctions.SpeedSlow(speed);
if (temp > easyLeftMembership)
{
easyLeftMembership = temp;
}
#endregion
#region straighttraight
double straightMembership;
temp = ElementaryFunctions.ReadingStraight(reading) * ElementaryFunctions.SpeedFast(speed);
straightMembership = temp;
temp = ElementaryFunctions.ReadingStraight(reading) * ElementaryFunctions.SpeedMedium(speed);
if (temp > straightMembership)
{
straightMembership = temp;
}
temp = ElementaryFunctions.ReadingStraight(reading) * ElementaryFunctions.SpeedSlow(speed);
if (temp > straightMembership)
{
straightMembership = temp;
}
#endregion
#region HardRight
double hardRightMembership;
temp = ElementaryFunctions.ReadingHardRight(reading) * ElementaryFunctions.SpeedFast(speed);
hardRightMembership = temp;
temp = ElementaryFunctions.ReadingHardRight(reading) * ElementaryFunctions.SpeedMedium(speed);
if (temp > hardRightMembership)
{
hardRightMembership = temp;
}
temp = ElementaryFunctions.ReadingEasyRight(reading) * ElementaryFunctions.SpeedFast(speed);
if (temp > hardRightMembership)
{
hardRightMembership = temp;
}
temp = ElementaryFunctions.ReadingHardRight(reading) * ElementaryFunctions.SpeedSlow(speed);
if (temp > hardRightMembership)
{
hardRightMembership = temp;
}
#endregion
#region EasyRight
double easyRightMembership;
temp = ElementaryFunctions.ReadingEasyRight(reading) * ElementaryFunctions.SpeedMedium(speed);
easyRightMembership = temp;
temp = ElementaryFunctions.ReadingEasyRight(reading) * ElementaryFunctions.SpeedSlow(speed);
if (temp > easyRightMembership)
{
easyRightMembership = temp;
}
#endregion
double value = (easyLeftMembership * easyLeft + easyRightMembership * easyRight + hardRightMembership * hardRight + hardLeftMembership * hardLeft + straightMembership * straight) / (easyLeftMembership + easyRightMembership + hardRightMembership + hardLeftMembership + straightMembership);
return(value);
}