Ultrasound Sensor: 2D Tracking With Arduino

1float  X,Y;
2void setup() {
3    Serial.begin(9600); // Starting Serial   Terminal
4
5    
6    pinMode(8, OUTPUT);   
7    pinMode(7, INPUT);
8     
9    pinMode(4, OUTPUT);
10    pinMode(6, INPUT);
11}
12
13void loop()   
14{
15Track();
16Serial.print(X);
17Serial.print("\	");
18Serial.println(Y);
19}
20
21
22
23//------------Function   For Object Track with Ultrasound------------//
24//this method use triangulates   Position of object by Using measurement 
25// of two ultrasound sensor saperated   by known distance.
26//output is based on mid point of two sensor
27//for more   details refer following link
28//
29//             by-ABHILASH
30
31void Track()
32{
33    float d1,d2,theta;
34   float dist=81; //distance between two sensors
35
36    //pins for sensor 1 need to be uploaded
37   digitalWrite(8, LOW);
38   delayMicroseconds(2);
39    digitalWrite(8, HIGH);
40   delayMicroseconds(10);
41   digitalWrite(8, LOW);
42    d1 = pulseIn(7, HIGH);
43   d1=d1*343/2000;
44
45  
46   delay(10);
47   //pins   for sensor 2 need to be uploaded
48   digitalWrite(4, LOW);
49   delayMicroseconds(2);
50    digitalWrite(4, HIGH);
51   delayMicroseconds(10);
52   digitalWrite(4, LOW);
53    d2 = pulseIn(6, HIGH);
54   d2=d2*343/2000;
55  
56theta=acos((((d1*d1)+(dist*dist)-(d2*d2)))/(2*d1*dist));
57
58if(theta<3   && theta>0){               // to avoid impossible values
59  X=d1*cos(theta)+ dist/2;   // y coordinate 
60  Y=d1*sin(theta); // X coordinate
61}
62else{Serial.println("bad   track");}
63/*Serial.print(d1);
64Serial.print("\	");
65Serial.print(d2);
66Serial.print("\	");
67Serial.print(theta*180/3.14);            //for debugging
68Serial.print("\	");
69Serial.print(X);
70Serial.print("\	");
71Serial.print(Y);
72Serial.print("\
73");*/
74delay(10);
75
76}
77

1float  X,Y;
2void setup() {
3    Serial.begin(9600); // Starting Serial  Terminal
4
5    
6    pinMode(8, OUTPUT);   
7    pinMode(7, INPUT);
8    
9    pinMode(4, OUTPUT);
10    pinMode(6, INPUT);
11}
12
13void loop()  
14{
15Track();
16Serial.print(X);
17Serial.print("\	");
18Serial.println(Y);
19}
20
21
22
23//------------Function  For Object Track with Ultrasound------------//
24//this method use triangulates  Position of object by Using measurement 
25// of two ultrasound sensor saperated  by known distance.
26//output is based on mid point of two sensor
27//for more  details refer following link
28//
29//             by-ABHILASH
30
31void Track()
32{
33   float d1,d2,theta;
34   float dist=81; //distance between two sensors
35
36   //pins for sensor 1 need to be uploaded
37   digitalWrite(8, LOW);
38   delayMicroseconds(2);
39   digitalWrite(8, HIGH);
40   delayMicroseconds(10);
41   digitalWrite(8, LOW);
42   d1 = pulseIn(7, HIGH);
43   d1=d1*343/2000;
44
45  
46   delay(10);
47   //pins  for sensor 2 need to be uploaded
48   digitalWrite(4, LOW);
49   delayMicroseconds(2);
50   digitalWrite(4, HIGH);
51   delayMicroseconds(10);
52   digitalWrite(4, LOW);
53   d2 = pulseIn(6, HIGH);
54   d2=d2*343/2000;
55  
56theta=acos((((d1*d1)+(dist*dist)-(d2*d2)))/(2*d1*dist));
57
58if(theta<3  && theta>0){               // to avoid impossible values
59  X=d1*cos(theta)+ dist/2;  // y coordinate 
60  Y=d1*sin(theta); // X coordinate
61}
62else{Serial.println("bad  track");}
63/*Serial.print(d1);
64Serial.print("\	");
65Serial.print(d2);
66Serial.print("\	");
67Serial.print(theta*180/3.14);           //for debugging
68Serial.print("\	");
69Serial.print(X);
70Serial.print("\	");
71Serial.print(Y);
72Serial.print("\
73");*/
74delay(10);
75
76}
77

1// below code is for target tracking in 2D using 2 ot more ultrasound  sensor
2//more details: https://youtu.be/MFVwW3jtXvw
3//by abhilash
4
5int  n=4;  //nos of sensor
6int Tx[4]={12,8,6,4};       //pin number: sensor 1, sensor  2,...
7int Rx[4]={11,7,5,3};       //pin number: sensor 1, sensor 2,...
8float  Raw[4];
9float Position[4]={0,41,85,130}; //all sensor midpoint are located in  one line only, 
10                                 // position mentoined from mid  point for left most sensor
11float Calc_X[6];        //for 4 sensor, total calculated  outpurt wil be 4C2=6 combinations
12float Calc_Y[6];
13int test[6];
14
15float  Final_X, Final_Y; // final calculated values wil be stored in these variables
16
17void  setup() 
18{
19      Serial.begin(250000); // Starting Serial Terminal
20
21        for(int i=0;i<n;i++)
22        {
23         pinMode(Tx[i], OUTPUT);   
24         pinMode(Rx[i], INPUT);
25        }
26}
27
28void loop() 
29{
30
31Raw_cap();
32Position_calc();
33Fuse_data();
34Serial.print(Final_X);  // X coordinate
35Serial.print("\	");
36Serial.println(Final_Y);
37
38}
39
40
41
42
43
44
45//==============================Functions==============================//
46
47
48
49//Raw_cap  is function to capture data from various sensor
50//by setting value of samples,  average of defined samples can be taken as output
51void Raw_cap()          
52{  
53  float sample=1;
54  for(int i=0; i<n; i++)
55        { 
56          Raw[i]=0;
57            for(int j=0; j<sample; j++)
58             {              
59                digitalWrite(Tx[i],  LOW);   
60                pinMode(Rx[i], INPUT);
61                digitalWrite(Tx[i],  LOW);
62                delayMicroseconds(2);
63                digitalWrite(Tx[i],  HIGH);
64                delayMicroseconds(10);
65                digitalWrite(Tx[i],  LOW);
66                Raw[i] =Raw[i] +  pulseIn(Rx[i], HIGH,5000);
67                delay(15);
68             }
69          Raw[i]=Raw[i]/sample;
70          Raw[i]=Raw[i]*343/2000;
71    //      Serial.println(Raw[i]);
72          
73        }
74}
75
76
77//-------------------------------------------------------------------------------------//
78//  this function applies tringle properties to find co-ordinates of 
79// various  combination of ultrasound sensor
80void Position_calc()
81{float d1,d2,a;
82int  count,test_temp;
83count=0;
84  for(int i=0;i<n;i++)
85  {
86    for(int j=i+1;j<n;j++)
87    {
88    d1=Raw[i]; d2=Raw[j]; a=Position[j]- Position[i];
89    test_temp=check(d1,d2,a);
90   
91      if(test_temp==0)
92        {      
93            Calc_X[count]=0;
94            Calc_Y[count]=0;
95            test[count]=0;        
96        }
97    
98      if(test_temp==1)
99        {
100   //  Serial.print(d1);Serial.print("\	");
101   //  Serial.print(d2);Serial.print("\	");
102   //  Serial.print(a);Serial.print("\	");
103          
104      //applying cosine rule to calculate angle    
105              float  theta=acos((((d1*d1)+(a*a)-(d2*d2)))/(2*d1*a));
106              Calc_X[count]=d1*cos(theta)+  Position[i]; // y coordinate 
107              Calc_Y[count]=d1*sin(theta);            //  X coordinate  
108              test[count]=1;  
109           //   Serial.print(Position[i]);Serial.print("\	");
110        }  
111 // Serial.print(Calc_X[count]);Serial.print("\	");
112 // Serial.print(Calc_Y[count]);Serial.print("\	");
113  // Serial.print(test[count]);Serial.print("\
114");
115
116      count=count+1;   
117    } 
118  }
119}
120
121
122
123//-------------------------------------------------------------------------------------//
124//  this function check if triangle is possible our not for selected combination of  sensor data
125int check(float d1,float d2,float a) 
126{
127  int temp_test;
128if(d1>d2+a  || d2>d1+a  ||  a>d2+d1){return 0;}
129else{return 1;}
130}
131
132
133//-------------------------------------------------------------------------------------//
134//this  function combines data of various sensor data combination with
135// weitage as  per accuracy of perticular combination
136// and proportional to distance in between  sensors
137void Fuse_data()
138{
139float temp_sum=0;
140int count=0;
141Final_X=0;
142Final_Y=0;
143for(int  i=0;i<n;i++)
144    {
145    for(int j=i+1;j<n;j++)
146      {
147        if(test[count]==1){temp_sum=temp_sum+Position[j]-Position[i];}
148   
149        Final_X=Final_X+Calc_X[count]*test[count]*(Position[j]-Position[i]);
150        Final_Y=Final_Y+Calc_Y[count]*test[count]*(Position[j]-Position[i]);
151        
152        count=count+1;
153      }
154    }
155    
156  Final_X=Final_X/temp_sum;
157  Final_Y=Final_Y/temp_sum;
158
159}
160