# Ultrasound Sensor: 2D Tracking With Arduino

Typically ultrasound sensor used to measure the range of the target, this project can locate the target in two dimensions.

BeginnerFull instructions provided10,851

## Things used in this project

### Hardware components

 Arduino UNO
×1
 Ultrasonic Sensor - HC-SR04 (Generic)
×4

### Software apps and online services

 Arduino IDE

## Code

### Multi sensor 2D tracking

Arduino
```// below code is for target tracking in 2D using 2 ot more ultrasound sensor
//more details: https://youtu.be/MFVwW3jtXvw
//by abhilash

int n=4;  //nos of sensor
int Tx[4]={12,8,6,4};       //pin number: sensor 1, sensor 2,...
int Rx[4]={11,7,5,3};       //pin number: sensor 1, sensor 2,...
float Raw[4];
float Position[4]={0,41,85,130}; //all sensor midpoint are located in one line only,
// position mentoined from mid point for left most sensor
float Calc_X[6];        //for 4 sensor, total calculated outpurt wil be 4C2=6 combinations
float Calc_Y[6];
int test[6];

float Final_X, Final_Y; // final calculated values wil be stored in these variables

void setup()
{
Serial.begin(250000); // Starting Serial Terminal

for(int i=0;i<n;i++)
{
pinMode(Tx[i], OUTPUT);
pinMode(Rx[i], INPUT);
}
}

void loop()
{

Raw_cap();
Position_calc();
Fuse_data();
Serial.print(Final_X);  // X coordinate
Serial.print("\t");
Serial.println(Final_Y);

}

//==============================Functions==============================//

//Raw_cap is function to capture data from various sensor
//by setting value of samples, average of defined samples can be taken as output
void Raw_cap()
{
float sample=1;
for(int i=0; i<n; i++)
{
Raw[i]=0;
for(int j=0; j<sample; j++)
{
digitalWrite(Tx[i], LOW);
pinMode(Rx[i], INPUT);
digitalWrite(Tx[i], LOW);
delayMicroseconds(2);
digitalWrite(Tx[i], HIGH);
delayMicroseconds(10);
digitalWrite(Tx[i], LOW);
Raw[i] =Raw[i] +  pulseIn(Rx[i], HIGH,5000);
delay(15);
}
Raw[i]=Raw[i]/sample;
Raw[i]=Raw[i]*343/2000;
//      Serial.println(Raw[i]);

}
}

//-------------------------------------------------------------------------------------//
// this function applies tringle properties to find co-ordinates of
// various combination of ultrasound sensor
void Position_calc()
{float d1,d2,a;
int  count,test_temp;
count=0;
for(int i=0;i<n;i++)
{
for(int j=i+1;j<n;j++)
{
d1=Raw[i]; d2=Raw[j]; a=Position[j]- Position[i];
test_temp=check(d1,d2,a);

if(test_temp==0)
{
Calc_X[count]=0;
Calc_Y[count]=0;
test[count]=0;
}

if(test_temp==1)
{
//  Serial.print(d1);Serial.print("\t");
//  Serial.print(d2);Serial.print("\t");
//  Serial.print(a);Serial.print("\t");

//applying cosine rule to calculate angle
float theta=acos((((d1*d1)+(a*a)-(d2*d2)))/(2*d1*a));
Calc_X[count]=d1*cos(theta)+ Position[i]; // y coordinate
Calc_Y[count]=d1*sin(theta);            // X coordinate
test[count]=1;
//   Serial.print(Position[i]);Serial.print("\t");
}
// Serial.print(Calc_X[count]);Serial.print("\t");
// Serial.print(Calc_Y[count]);Serial.print("\t");
// Serial.print(test[count]);Serial.print("\n");

count=count+1;
}
}
}

//-------------------------------------------------------------------------------------//
// this function check if triangle is possible our not for selected combination of sensor data
int check(float d1,float d2,float a)
{
int temp_test;
if(d1>d2+a || d2>d1+a  ||  a>d2+d1){return 0;}
else{return 1;}
}

//-------------------------------------------------------------------------------------//
//this function combines data of various sensor data combination with
// weitage as per accuracy of perticular combination
// and proportional to distance in between sensors
void Fuse_data()
{
float temp_sum=0;
int count=0;
Final_X=0;
Final_Y=0;
for(int i=0;i<n;i++)
{
for(int j=i+1;j<n;j++)
{
if(test[count]==1){temp_sum=temp_sum+Position[j]-Position[i];}

Final_X=Final_X+Calc_X[count]*test[count]*(Position[j]-Position[i]);
Final_Y=Final_Y+Calc_Y[count]*test[count]*(Position[j]-Position[i]);

count=count+1;
}
}

Final_X=Final_X/temp_sum;
Final_Y=Final_Y/temp_sum;

}
```

### Tracking with 2 sensor

Arduino
```float  X,Y;
void setup() {
Serial.begin(9600); // Starting Serial Terminal

pinMode(8, OUTPUT);
pinMode(7, INPUT);

pinMode(4, OUTPUT);
pinMode(6, INPUT);
}

void loop()
{
Track();
Serial.print(X);
Serial.print("\t");
Serial.println(Y);
}

//------------Function For Object Track with Ultrasound------------//
//this method use triangulates Position of object by Using measurement
// of two ultrasound sensor saperated by known distance.
//output is based on mid point of two sensor
//for more details refer following link
//
//             by-ABHILASH

void Track()
{
float d1,d2,theta;
float dist=81; //distance between two sensors

//pins for sensor 1 need to be uploaded
digitalWrite(8, LOW);
delayMicroseconds(2);
digitalWrite(8, HIGH);
delayMicroseconds(10);
digitalWrite(8, LOW);
d1 = pulseIn(7, HIGH);
d1=d1*343/2000;

delay(10);
//pins for sensor 2 need to be uploaded
digitalWrite(4, LOW);
delayMicroseconds(2);
digitalWrite(4, HIGH);
delayMicroseconds(10);
digitalWrite(4, LOW);
d2 = pulseIn(6, HIGH);
d2=d2*343/2000;

theta=acos((((d1*d1)+(dist*dist)-(d2*d2)))/(2*d1*dist));

if(theta<3 && theta>0){               // to avoid impossible values
X=d1*cos(theta)+ dist/2; // y coordinate
Y=d1*sin(theta); // X coordinate
}
/*Serial.print(d1);
Serial.print("\t");
Serial.print(d2);
Serial.print("\t");
Serial.print(theta*180/3.14);           //for debugging
Serial.print("\t");
Serial.print(X);
Serial.print("\t");
Serial.print(Y);
Serial.print("\n");*/
delay(10);

}
```

## Credits

### abhilash_patel

0 projects • 26 followers