Странное поведение метода для второго запуска

Я пишу программу Arduino, которая использует нечеткую логику для управления автомобилем, но у меня есть проблемы с C ++, так как программа ведет себя очень странно. Первый прогон дает мне ПРАВИЛЬНЫЕ результаты, но следующий прогон смещает выходные данные в массиве действительно странным образом.

РЕДАКТИРОВАТЬ: Я обнаружил, что проблема связана с количеством FSets, хранящихся в TermSet. Если я добавлю в TermSet более 5 FSets, ИТ даст НЕПРАВИЛЬНЫЕ результаты только на 3-й, 4-й и 5-й итерации. Другие итерации дают правильные результаты.

Мой код выполнения:

// 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);

}

Это выводит что-то вроде этого для 3 прогонов:
// cntZ для целого UOD — каждый прогон корректен — исключен из вывода в следующих прогонах
-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.00RUN 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

Код классов:

//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;
}

Я ожидаю, что проблема в FRule->ruleOutput(double x1,double x2) метод, но я могу ошибаться. Также я думаю, что это должно быть какая-то проблема с памятью, так как я новичок в C ++ и не совсем доволен всем управлением памятью.