66 lines
2.1 KiB
C++
66 lines
2.1 KiB
C++
#ifndef FUZZY_H
|
|
#define FUZZY_H
|
|
|
|
#include <Arduino.h>
|
|
#include <algorithm>
|
|
|
|
// Rear servo configuration
|
|
const int REAR_MIN = 52;
|
|
const int REAR_MAX = 140;
|
|
const int REAR_STRAIGHT = 95;
|
|
|
|
// Fuzzy logic functions
|
|
float triangularMF(float x, float a, float b, float c) {
|
|
return std::max(std::min((x - a) / (b - a), (c - x) / (c - b)), 0.0f);
|
|
}
|
|
|
|
float calculateCentroid(float* memberships, float* angles, int size) {
|
|
|
|
float sumMembership = 0;
|
|
float sumProduct = 0;
|
|
for (int i = 0; i < size; i++) {
|
|
sumMembership += memberships[i];
|
|
sumProduct += memberships[i] * angles[i];
|
|
}
|
|
|
|
return sumProduct / sumMembership;
|
|
|
|
}
|
|
|
|
float fuzzyRearSteering(float accelX, float accelY) {
|
|
|
|
// Define membership functions for inputs
|
|
float accelNeg = triangularMF(accelX, -1, -0.5, 0);
|
|
float accelZero = triangularMF(accelX, -0.5, 0, 0.5);
|
|
float accelPos = triangularMF(accelX, 0, 0.5, 1);
|
|
|
|
float rotSmall = triangularMF(accelY, -1, -0.5, 0);
|
|
float rotMedium = triangularMF(accelY, -0.5, 0, 0.5);
|
|
float rotLarge = triangularMF(accelY, 0, 0.5, 1);
|
|
|
|
// Define rule base
|
|
float ruleStrengths[9];
|
|
ruleStrengths[0] = std::min(accelNeg, rotSmall); // Right
|
|
ruleStrengths[1] = std::min(accelNeg, rotMedium); // Straight
|
|
ruleStrengths[2] = std::min(accelNeg, rotLarge); // Left
|
|
ruleStrengths[3] = std::min(accelZero, rotSmall); // Straight
|
|
ruleStrengths[4] = std::min(accelZero, rotMedium); // Straight
|
|
ruleStrengths[5] = std::min(accelZero, rotLarge); // Left
|
|
ruleStrengths[6] = std::min(accelPos, rotSmall); // Left
|
|
ruleStrengths[7] = std::min(accelPos, rotMedium); // Straight
|
|
ruleStrengths[8] = std::min(accelPos, rotLarge); // Right
|
|
|
|
// Define output angles (adjusted for rear servo range)
|
|
float outputAngles[9] = {
|
|
REAR_MAX, REAR_STRAIGHT, REAR_MIN, // Right, Straight, Left
|
|
REAR_STRAIGHT, REAR_STRAIGHT, REAR_MIN, // Straight, Straight, Left
|
|
REAR_MIN, REAR_STRAIGHT, REAR_MAX // Left, Straight, Right
|
|
};
|
|
|
|
// Defuzzification using centroid method
|
|
return calculateCentroid(ruleStrengths, outputAngles, 9);
|
|
|
|
}
|
|
|
|
#endif
|