FN
Dependencies: mbed RF24Network RF24
Diff: main.cpp
- Revision:
- 9:ec9bf9f7645f
- Parent:
- 8:b0775f476afc
- Child:
- 10:09b236addeb5
diff -r b0775f476afc -r ec9bf9f7645f main.cpp --- 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); +}