Creating clean array everytime function is run in Arduino code - c++

Question

I'm writing a program which gets the distances from five ultrasonic sensors and uses a trilateration algorithm on every combination of distance values. It first outputs all the coordinates it recieves from the trilateration algorithm, then it filters out values which are too small or too large. However, it keeps outputting values from previous trilateration cycles as the array has not been reset.
#include "NewPing.h"
#include<stdio.h>
#include<stdlib.h>
#define TP0 11
#define EP0 13
#define TP1 3
#define EP1 4
#define TP2 6
#define EP2 2
#define TP3 5
#define EP3 7
#define TP4 10
#define EP4 8
#define MAX_DISTANCE 150
//sensor coordinates
int xcords [5] = {3, 45, 87, 87, 3};
int ycords [5] = {0, 16, 0, 58, 58};
int r = 3;
int n = 5;
float x = 0;
float y = 0;
int MAX_X = 85;
int MAX_Y = 62;
int sensor;
int count = 0;
int resultCount = 0;
bool reading = false;
void setup() {
Serial.begin (9600);
}
void
loop() {
while(count < 1){
count += 1;
int distances [3];
for (int r = 0; r<5; r++){
distances[r] = getValue(r);
if (distances[r] >= MAX_DISTANCE || distances[r] <= 2) {
getValue(r);
}
//Serial.print("Distance =");
//Serial.print(distances[r]);
//Serial.println(" cm ");
}
printCombination(distances, xcords, ycords, n, r);
//trilateration(distances[0], distances[1], distances[2]);
delay(5000);
}
}
int combinationUtil(int distances[], int data[], int dataCords[], int start, int end, int index, int r, float xresults[], float yresults[]);
// Needed for qsort. See http://w...content-available-to-author-only...s.com/reference/cstdlib/qsort/
int compare (const void * a, const void * b)
{ return ( *(int*)a - *(int*)b ); }
// The main function that prints all combinations of size r
// in arr[] of size n. This function mainly uses combinationUtil()
int printCombination(int distances[], int xcords[], int ycords[], int n, int r)
{
// A temporary array to store all combination one by one
int data[r];
int xdataCords[r];
int ydataCords[r];
float xresults[10];// the arrays causing the issue
float yresults[10];
// Sort array to handle duplicates
qsort (distances, n, sizeof(int), compare);
// Print all combination using temprary array 'data[]'
combinationUtil(distances, data, xdataCords, ydataCords, 0, n-1, 0, r, xresults, yresults);
sorting(xresults,yresults);
resultCount = 0;
Serial.print("\n");
}
/* distances[] ---> Input Array
data[] ---> Temporary array to store current combination
start & end ---> Staring and Ending indexes in distances[]
index ---> Current index in data[]
r ---> Size of a combination to be printed */
float* combinationUtil(int distances[], int data[], int xdataCords[], int ydataCords[], int start, int end, int index, int r, float xresults[], float yresults[])
{
// Current combination is ready to be printed, print it
if (index == r)
{
trilateration(data[0], data[1], data[2], xdataCords[0], xdataCords[1], xdataCords[2], ydataCords[0], ydataCords[1], ydataCords[2], xresults, yresults);
resultCount++;
}
// replace index with all possible elements. The condition
// "end-i+1 >= r-index" makes sure that including one element
// at index will make a combination with remaining elements
// at remaining positions
for (int i=start; i<=end && end-i+1 >= r-index; i++)
{
data[index] = distances[i];
xdataCords[index] = xcords[i];
ydataCords[index] = ycords[i];
combinationUtil(distances, data, xdataCords, ydataCords, i+1, end, index+1, r, xresults, yresults);
// Remove duplicates
while (distances[i] == distances[i+1])
i++;
}
}
float* trilateration(int d1,int d2, int d3, int x1, int x2, int x3, int y1, int y2, int y3, float xresults[], float yresults[]){
float va = ((sq(d2) - sq(d3)) - (sq(x2) - sq(x3)) - (sq(y2) - sq(y3))) / (2) ;
float vb = ((sq(d2) - sq(d1)) - (sq(x2) - sq(x1)) - (sq(y2) - sq(y1))) / (2) ;
y = ((vb*(x3-x2)) - (va*(x1-x2))) / (((y1-y2)*(x3-x2)) - ((y3-y2)*(x1-x2)));
x = (va - (y*(y3-y2))) / (x3-x2);
//Serial.print(d1);
//Serial.print(d2);
//Serial.print(d3);
Serial.print("The coordinates are: ");
Serial.print(abs(x));
xresults[resultCount] = abs(x);
Serial.print(", ");
Serial.println(abs(y));
yresults[resultCount] = abs(y);
//sorting(xresults, yresults);
return xresults, yresults;
}
void sorting(float xresults[], float yresults[]){
float xfinal[10];
float yfinal[10];
qsort (xresults, 10, sizeof(int), compare);
qsort (yresults, 10, sizeof(int), compare);
for (int i = 0; i<10; i++){
if (xresults[i] > 3 && xresults[i] < MAX_X){
xfinal[i] = xresults[i];
Serial.print("x=");
Serial.print(xresults[i]);
Serial.print("\n");
}
if (yresults[i] > 10 && yresults[i] < MAX_Y){
yfinal[i] = yresults[i];
Serial.print("y=");
Serial.print(yresults[i]);
Serial.print("\n");
}
}
}
float getValue(int sensor){
NewPing sonar0(TP0, EP0, MAX_DISTANCE);
NewPing sonar1(TP1, EP1, MAX_DISTANCE);
NewPing sonar2(TP2, EP2, MAX_DISTANCE);
NewPing sonar3(TP3, EP3, MAX_DISTANCE);
NewPing sonar4(TP4, EP4, MAX_DISTANCE);
switch(sensor){
case 0:
return sonar0.ping_cm();
case 1:
return sonar1.ping_cm();
case 2:
return sonar2.ping_cm();
case 3:
return sonar3.ping_cm();
case 4:
return sonar4.ping_cm();
}
}
the arrays "xresults and yresults" is the array causing the problem. You can see that I have redefined it every time printCombination is called so it should be cleared right?
This is my output:
The coordinates are: 40.33, 34.88
The coordinates are: 58.62, 8.00
The coordinates are: 42.09, 8.00
The coordinates are: nan, 7.74
The coordinates are: 59.14, 24.17
The coordinates are: 47.81, 13.41
The coordinates are: 47.81, 7.74
x=40.25
x=58.56
y=24.16
x=47.81
y=13.41
x=47.81
x=32.99
New distance call
The coordinates are: 40.65, 35.72
The coordinates are: 58.62, 8.00
The coordinates are: 42.09, 8.00
The coordinates are: nan, 7.28
The coordinates are: 60.07, 25.03
The coordinates are: 47.81, 13.41
The coordinates are: 47.81, 7.28
x=40.50
x=58.56
y=35.56
x=47.81
y=13.41
x=47.81
x=32.99
New distance call
The coordinates are: 24.98, 15.69
The coordinates are: 50.48, 8.00
The coordinates are: 112.35, 8.00
The coordinates are: nan, 25.91
The coordinates are: 14.66, 84.64
The coordinates are: 91.00, 12.26
The coordinates are: 91.00, 25.91
x=14.63
x=50.31
y=15.70
y=12.26
y=25.91
x=32.99
As you can see there are values which are not in the list of coordinates appearing in the print out after the coordinates.