Revision 9:ec9bf9f7645f, committed 2019-04-08

Comitter:

belkaous

Date:

Mon Apr 08 12:51:46 2019 +0000

Parent:

8:b0775f476afc

Child:

10:09b236addeb5

Commit message:

FN

Changed in this revision

--- a/main.cpp	Mon Apr 08 12:19:10 2019 +0000
+++ b/main.cpp	Mon Apr 08 12:51:46 2019 +0000
@@ -5,7 +5,7 @@
 Serial pc(USBTX, USBRX);
 InterruptIn button(D8);
 AnalogIn LM35(A2);
-    
+void position_abs(double, double, double, double,double);
 
 RF24 radio(SPI_MOSI, SPI_MISO, SPI_SCK, D9, SPI_CS );
 Timer timer;
@@ -59,8 +59,7 @@
     unsigned long counter;
 };
  
- 
- void emmeteur(){
+void emmeteur(){
 
             pc.printf("Sending : %d...",indice+1);
             payload_t payload_tx;
@@ -94,14 +93,8 @@
                // a=1;
                 }
             else
-                pc.printf("failed : 2.\r\n");
-     
-     
+                pc.printf("failed : 2.\r\n");     
 }
- 
- 
- 
- 
 int main()
 {
   //  double cpt=0;
@@ -119,11 +112,7 @@
 //    for (int i=0;i<10;i++) {
 //        temperature += LM35.read();
 //        wait_ms(10);
-//    }
-    
-    
-    
-    
+//    }  
   //  temperature = temperature*34;
     
     vitesse = 342.0;//331.5 + 0.595*temperature;
@@ -141,9 +130,10 @@
         {
             if(indice > 2) {
                 for(int j=0; j<n; j++)
-                    printf("distance %d = %f\r\n",j+1,d[j]);
+                printf("distance %d = %f\r\n",j+1,d[j]);
+                position_abs(610.0,600.0,d[0]*1000,d[1]*1000,d[2]*1000);
                 indice = 0;
-            }
+        }
             t.reset();
             
             button.enable_irq();
@@ -152,16 +142,63 @@
 
             indice++;
         }
-       
-       
-        //}
-           
-            
-            
+    }
+}
+
+void position_abs(double L, double l, double d1, double d2, double d3)
+{
+        printf("Hellooooooo auj!! \r\n");
+        
+        double w = l; /* dim terrain */
+        double u = sqrt(pow((double)  L,2)+pow((double) l/2,2)), v = u;
+        double q = acos((w/2)/v), r = 18.44, t=q;
+        
+        /* triangle (B1,R,B3) */
+        
+        double k = acos((pow(d1,2)+pow(v,2)-pow(d3,2))/(2*d1*v));
+        //printf("k = %lf \n\r",k);
+        double alpha = acos((pow(d1,2)+pow(d3,2)-pow(v,2))/(2*d1*d3));
+        //printf("alpha = %lf \n\r",alpha);
+        double p = acos((pow(v,2)+pow(d3,2)-pow(d1,2))/(2*v*d3));
+        //printf("p = %lf \n\r",p);
+        
+        /* triangle (B2,R,B3)*/
+        
+        double O = acos((pow(d3,2)+pow(u,2)-pow(d2,2))/(2*d3*u));
+        //double O = acos((pow(d3,2)+pow(d2,2)-pow(u,2))/(2*d3*d2));
+        //printf("O = %lf \n\r",O);
+        double m = acos((pow(u,2)+pow(d2,2)-pow(d3,2))/(2*u*d2));
+        //double m = acos((pow(d3,2)+pow(u,2)-pow(d2,2))/(2*u*d3));
+        //printf("m = %lf \n\r",m);
+        /* triangle (B1,R,B2) */
         
-       
- 
- 
-    }
- 
-}
\ No newline at end of file
+        double i = acos(((double) pow((double) w,2)+(double) pow((double) d1,2)-(double) pow((double) d2,2))/(double) (2*w*d1));
+        //printf(" i= %lf \n\r",i);
+        double j = acos(((double) pow((double) d2,2)+(double) pow((double) w,2)-(double) pow((double) d1,2))/(double) (2*w*d2));
+        //printf(" j= %lf \n\r",j);
+        
+        /* triangle (B1,R,B3) */
+        double x1 = w/2 + cos(3.14-(p+q))*d3;
+        //printf(" x1= %lf \n\r",x1);
+        double y1 = sin(3.14-(p+q))*d3;
+        //printf(" y1= %lf \n\r",y1);
+        
+        /* triangle (B1,R,B2) */
+        
+        double x2 = cos(i) *d1;
+        //printf(" x2= %lf \n\r",x2);
+        double y2 = L - sin(i)*d1;
+        //printf(" y2= %lf \n\r",y2);
+        
+        /* triangle (B2,R,B3)*/
+        
+        double x3 = w/2 + cos(O+t)*d3;
+        //printf(" x3= %lf \n\r",x3);
+        double y3 = sin(O+t)*d3;
+        //printf(" y3= %lf \n\r",y3);
+        
+        double xt = (x1+x2+x3)/3;
+        printf(" xt= %lf \r\n",xt);
+        double yt = (y1+y2+y3)/3;
+        printf(" yt= %lf \r\n",yt);
+}