I am writing an Arduino program that use a fuzzy logic to control the car, but I have problems with the C++, as the program behaves really strange. The first run gets me RIGHT results, but next runs shifts outputs in the array really strange way.
EDIT: I found out, that the problem is related to the number of FSets stored in the TermSet. If I put into TermSet more than 5 FSets, IT gives WRONG results only in the 3rd, 4th and 5th iteration. Other iterations gives RIGHT results.
My execution code is:
// Setup the FSets
FSet errZ(0,2,0);
FSet errDtZ(0,0.5,0);
FSet cntZ(0,4,0);
//Setup the TermSets
TermSet errDt(-1,1,0.1,1);
TermSet err(-3,3,1,1);
TermSet cnt(-6,6,0.5,1);
void loop(){
//Fill the TermSets with FSets
int ierrDt = errDt.addFSet(&errDtZ);
int ierr = err.addFSet(&errZ);
int icnt = cnt.addFSet(&cntZ);
//Create a single FRule for test
//Passing in the errZ, errDtZ and cntZ FRules
FRule rule1(&err,ierr,&errDt,ierrDt,&cnt,icnt);
//Test if the cntZ FRule gives every loop correct answers
// It shows up, that it does! - not included in output
int j = 0;
for(float i = -6; i <= 6; i = i + 0.5){
Serial.print(i);
Serial.print("/");
Serial.print(cnt.getUODValue(j++));
Serial.print(" - ");
Serial.println(cntZ.lom(i));
}
double x1 = -1.5;
double x2 = 0.25;
//
Serial.println("===== SUMMARY =====");
// Call lom on FSets manualy - gives RIGHT RESULTS
Serial.print("LOM1: ");
Serial.println(errZ.lom(x1));
Serial.print("LOM2: ");
Serial.println(errDtZ.lom(x2));
Serial.println("===== END OF SUMMARY =====");
// Get the result of the rule - that gives sometime WRONG results
double* output = rule1.ruleOutput(x1,x2);
// Output the returned array
Serial.println();
Serial.println("------- Rule outputs -------");
for(int i = 0; i < cnt.UODLength(); i++){
Serial.print(cnt.getUODValue(i)); // Universe of discourse value X
Serial.print(" - ");
Serial.println(output[i]); // Outputed value for the givend X that belongs to UOD
}
delay(5000);
}
This outputs something like this for 3 runs:
// cntZ for WHOLE UOD - every run are correct - omitted from output in next runs
-6.00/-6.00 - 0.00
-5.00/-5.00 - 0.00
-4.00/-4.00 - 0.00
-3.00/-3.00 - 0.25
-2.00/-2.00 - 0.50
-1.00/-1.00 - 0.75
0.00/0.00 - 1.00
1.00/1.00 - 0.75
2.00/2.00 - 0.50
3.00/3.00 - 0.25
4.00/4.00 - 0.00
5.00/5.00 - 0.00
6.00/6.00 - 0.00
RUN 1
===== SUMMARY =====
LOM1: 0.88 //THESE VALUES ARE ALWAYS OK
LOM2: 0.80
===== END OF SUMMARY =====
LOM1 internal: 0.88 //THIS VALUE IS SOMETIMES WRONG - WHY?
LOM1 internal: 0.80
DOF internal: 0.80
LOM in -6.00 is 0.00
And output[i] is: 0.00
LOM in -5.00 is 0.00
And output[i] is: 0.00
LOM in -4.00 is 0.00
And output[i] is: 0.00
LOM in -3.00 is 0.25
And output[i] is: 0.25
LOM in -2.00 is 0.50
And output[i] is: 0.50
LOM in -1.00 is 0.75
And output[i] is: 0.75
LOM in 0.00 is 1.00
And output[i] is: 0.80
LOM in 1.00 is 0.75
And output[i] is: 0.75
LOM in 2.00 is 0.50
And output[i] is: 0.50
LOM in 3.00 is 0.25
And output[i] is: 0.25
LOM in 4.00 is 0.00
And output[i] is: 0.00
LOM in 5.00 is 0.00
And output[i] is: 0.00
LOM in 6.00 is 0.00
And output[i] is: 0.00
------- Rule outputs -------
-6.00 - 0.00
-5.00 - 0.00
-4.00 - 0.00
-3.00 - 0.25
-2.00 - 0.50
-1.00 - 0.75
0.00 - 0.80
1.00 - 0.75
2.00 - 0.50
3.00 - 0.25
4.00 - 0.00
5.00 - 0.00
6.00 - 0.00
RUN 2
===== SUMMARY =====
LOM1: 0.88 //OK
LOM2: 0.80
===== END OF SUMMARY =====
LOM1 internal: 0.88 //OK
LOM1 internal: 0.80
DOF internal: 0.80
//HERE IT STARTS TO BE QUITE STRANGE
LOM in -6.00 is 0.00
And output[i] is: 0.00
LOM in -5.00 is 85827176.00
And output[i] is: 0.80
LOM in -4.00 is 107218432.00
And output[i] is: 0.80
LOM in -3.00 is 142783152.00
And output[i] is: 0.80
LOM in -2.00 is 213912576.00
And output[i] is: 0.80
LOM in -1.00 is 428873696.00
And output[i] is: 0.80
LOM in 0.00 is -0.00
And output[i] is: -0.00
LOM in 1.00 is 0.00
And output[i] is: 0.00
LOM in 2.00 is 0.00
And output[i] is: 0.00
LOM in 3.00 is 0.00
And output[i] is: 0.00
LOM in 4.00 is 0.00
And output[i] is: 0.00
LOM in 5.00 is 0.00
And output[i] is: 0.00
LOM in 6.00 is 0.00
And output[i] is: 0.00
------- Rule outputs -------
//RULE OUTPUTS TOTALLY MIXED UP
-6.00 - 0.00
-5.00 - 0.80
-4.00 - 0.80
-3.00 - 0.80
-2.00 - 0.80
-1.00 - 0.80
0.00 - -0.00
1.00 - 0.00
2.00 - 0.00
3.00 - 0.00
4.00 - 0.00
5.00 - 0.00
6.00 - 0.00
RUN 3
===== SUMMARY =====
LOM1: 0.88
LOM2: 0.80
===== END OF SUMMARY =====
LOM1 internal: 0.00 //THAT IS WRONG VALUE
LOM1 internal: 0.80
DOF internal: 0.00
//WRONG AGAIN
LOM in -6.00 is 0.00
And output[i] is: 0.00
LOM in -5.00 is 0.00
And output[i] is: 0.00
LOM in -4.00 is 0.00
And output[i] is: 0.00
LOM in -3.00 is 0.00
And output[i] is: 0.00
LOM in -2.00 is 0.00
And output[i] is: 0.00
LOM in -1.00 is 0.00
And output[i] is: 0.00
LOM in 0.00 is 0.00
And output[i] is: 0.00
LOM in 1.00 is 0.00
And output[i] is: 0.00
LOM in 2.00 is 0.00
And output[i] is: 0.00
LOM in 3.00 is 0.00
And output[i] is: 0.00
LOM in 4.00 is 0.00
And output[i] is: 0.00
LOM in 5.00 is 0.00
And output[i] is: 0.00
LOM in 6.00 is 0.00
And output[i] is: 0.00
//WRONG
------- Rule outputs -------
-6.00 - 0.00
-5.00 - 0.00
-4.00 - 0.00
-3.00 - 0.00
-2.00 - 0.00
-1.00 - 0.00
0.00 - 0.00
1.00 - 0.00
2.00 - 0.00
3.00 - 0.00
4.00 - 0.00
5.00 - 0.00
6.00 - 0.00
The classes code is:
//Class representing the fuzzy set
FSet::FSet(double center, double delta, int inf){
this->inf = inf;
this->delta = delta;
this->a = center - delta;
this->b = center + delta;
this->center = center;
}
//Return the level of matching of some crisp value
double FSet::lom(double crispValue) {
if(inf == 0){
if((crispValue < this->a) || (crispValue > this->b)){
return 0.0;
} else {
return 1 - abs(crispValue-center)*(1.0/delta);
}
} else {
if( ((crispValue < this->a) && (inf > 0)) ||
((crispValue > this->b) && (inf < 0))){
return 0.0;
}
if((inf > 0) && (crispValue < center)){
return 1 - abs(crispValue-center)*(1.0/delta);
} else if((inf < 0) && (crispValue > center)){
return 1 - abs(crispValue-center)*(1.0/delta);
} else {
return 1.0;
}
}
}
/*
* Class that contains all the FuzzySets of the same variable
* e.g.: TermSet err would include negativeErro, zeroError, positiveError etc.
*/
TermSet::TermSet(double a, double b, double step, int setsSize){
//Boundaries of the Universe of discourse (UOD)
this->a = a;
this->b = b;
//Step for discretization of the UOD
this->step = step;
//Index of the last inserted FSet
this->last = -1;
//Number of FSets
this->setsSize = setsSize;
this->sets = (FSet**) malloc(sizeof(FSet*) * setsSize);
// The length of the universe of discourse
this->length = (b-a)/step + 1;
}
TermSet::~TermSet(){
free(this->sets);
}
// Return the FSet by index
FSet* TermSet::getFSet(int index){
return this->sets[index];
}
/* Adds FSet to the TermSet
* return the index of the FSet in the array
*/
int TermSet::addFSet(FSet* set){
sets[++last] = set;
return last;
}
/* Get the value of the UOD that is n steps
* right of the begging of the UOD
*/
double TermSet::getUODValue(int n){
return a + n*step;
}
// Return the length of the UOD
int TermSet::UODLength(){
return length;
}
/*
* Class representing the FuzzyRule of form:
* IF error is positive AND change of error is positive THEN change of control is negative
* Which in code is represented as:
* IF x1 is ant1[U1] AND x2 is ant2[U2] THEN y is con[D]
*/
FRule::FRule(TermSet* ant1, int u1, TermSet* ant2, int u2, TermSet* conTermSet, int d){
// TermSet for the antecedent part of the rule
this->ant1 = ant1;
this->ant2 = ant2;
//Indexes of the FRules in the antacendents ant1 and ant2
this->U1 = u1;
this->U2 = u2;
//TermSet for the consequent part
this->con = conTermSet;
//Index of the consequent FRule in the con TermSet
this->D = d;
// Array of the output values
output = (double*) malloc(sizeof(double) * con->UODLength());
}
FRule::~FRule(){
free(output);
}
/* Get the degree of firing of the antacendent part
* that is return the min of the FSet->lom(x) values;
*/
double FRule::dof(double x1,double x2){
double pi1 = ant1->getFSet(U1)->lom(x1);
double pi2 = ant2->getFSet(U2)->lom(x2);
Serial.print("LOM1 internal: ");
Serial.println(pi1);
Serial.print("LOM1 internal: ");
Serial.println(pi2);
return min(pi1,pi2);
}
double* FRule::ruleOutput(double x1, double x2){
// Get the degree of firing of the antacendent part
double adof = this->dof(x1,x2);
Serial.print("DOF internal: ");
Serial.println(adof);
/* Iterate throw the universe of discourse of the consequent part and
* combine the lom of the consequent with value with the dof value of
* antacendent using the min operator
*/
double j = con->a;
for (int i = 0; i < con->UODLength(); i++){
double dlom = con->getFSet(D)->lom(j);
Serial.print("LOM in ");
Serial.print(j);
Serial.print(" is ");
Serial.println(dlom);
output[i] = min(adof,dlom);
j = j + con->step;
Serial.print("And output[i] is: ");
Serial.println(output[i]);
}
return output;
}
I expect that the problem is in the FRule->ruleOutput(double x1,double x2) method, but I might be wrong. Also I think it must be some sort of memory problem, as I am new to C++ and not quite comfortable with the whole memory management.