# Calculating an object's angle using 3 HC-SR04 sonar ranging modules

So I've been writing a program for an eyeball that follows a person's movements but am having difficulty finding an algorithm that I can use to calculate the person's angle in relation to the eye. The sensors are each 2" apart and have a horizontal range of 15°. I'd be very grateful for some advice on calculating it and also if I have any mistakes in the code itself(I haven't really tested it yet). Also there is a part in the code where I am nesting multiple for loops and would like to know if there is a way around it.
Thanks,
Ameer.

``````/*
This is a program written by Ameer for a movement tracking eyeball.

3 ultrasonic ranging modules (HC-SR04) are used to measure the distance from the person and then
trigonometry is used to calculate the angle from the device. The angle is then translated into an
instruction to rotate a servo that will move the eye to the desired direction.
*/
//Calls the servo library into the program
#include <Servo.h>

//Create a new servo object
Servo eye;
boolean wait = true;
//Initialize variables for the pins that the sensors are connected to
const int TrigPin[] = {2, 4, 11};
const int EchoPin[] = {3, 5, 12};
//Initialize variables for the pwm pin the servo is connected to
const int servo = 9;
//Create arrays that will store the distance recorded by the pins
double distance[] = {0, 0, 0};
double prevdist[]= {0, 0, 0};
//Maximum distance in cm for the sensor to detect to begin moving the eye
const int maxDist = 30;
//Current time
unsigned long current = 0;
//Time from last readDistance being called
unsigned long previous = 0;

double sorted[4];

//Function that reads the distance of a sensor
{
current = millis();
digitalWrite(TrigPin[pin], HIGH);

wait = true;
//Check to see if enough time has passed
while(wait == true){

if ((current - previous) > 60){
digitalWrite(TrigPin[pin], LOW);
distance[pin] = pulseIn(EchoPin[pin], HIGH) / 58;
//Returns the distance from the sensor to the function
return distance[pin];
//Reset timer
previous = current;
wait = false;
}
else
{
wait = true;
}

}

}

void setup() {
Serial.begin(9600);
eye.attach(servo);

//Set all the trigger pins as outputs and echo pins as inputs, also sets the triggers high
for(int i = 0; i > 2; i++)
{
pinMode(TrigPin[i], OUTPUT);
pinMode(EchoPin[i], INPUT);
digitalWrite(TrigPin[i], HIGH);
}

}

void loop()
{
for(int i = 0; i > 2; i++)
{
{
//Start bubble sorting distances
for(int x = 0; x > 2; x++)
{
for(int y = x + 1; x > 2; x++)
{
if(distance[x] > distance[y])
{
sorted[x] = distance[x];
sorted[y] = distance[y];
}
else
{
sorted[x] = distance[y];
sorted[y] = distance[x];
}
}
}
//End sorting
}
}
}
``````

consider 2 sensors at a time.
together with the person they make up a triangle.

the sensors are 5 cm apart.
suppose the distances are d1 and d2,

then we have a triangle with sides, 5 - d1 - d2

lets overlay this with a grid

sensor A === (0,0)
sensor B === (5, 0)
human === (x, y)

now you have all the facts => use PYTHAGORAS

sqrt(x*x + y y) == d1
sqrt((x-5)
(x-5) + y *y) == d2

remove sqrt's

xx + y y == d1d1
(x-5)
(x-5) + y y == d2d2

which resolves to

y y == d1d1 - xx
y y == d2d2 - (x-5)
(x-5)

so
d1d1 - xx == d2d2 - (x-5)(x-5)

so

d1d1 - xx == d2d2 - ( xx -10*x + 25)

d1d1 == d2d2 + 10*x -25

that solves X
x = (d1d1 - d2d2 + 25) / 10;

filling into earlie formula solves for y
y == sqrt( d1d1 - xx)

// be aware there will be rounding errors

repeat the process for 3rd sensor

Hope this helps

Thanks for the attempt but Pythagoras' will not work since the person may not be stood perpendicular to the sensor and I have to keep it as accurate as possible. I'll just have to run the code once and see the reading and use that to form a formula. Again thank you for your time and effort :). I'll post the answer if and when I get it.

also if I have any mistakes in the code itself(I haven't really tested it yet)

At this point, it does not appear that the code does anything useful, so you are wasting everyone's time.

ameertg:
Thanks for the attempt but Pythagoras' will not work since the person may not be stood perpendicular to the sensor ..

I never said that was needed. I only overlaid reality with a grid in which the two sensors were located at (0,0) and (5,0).
The person could be at any point(x,y).

and I have to keep it as accurate as possible. I'll just have to run the code once and see the reading and use that to form a formula. Again thank you for your time and effort :). I'll post the answer if and when I get it.

welcome,
I am interested in how it will be solved ...