Revision 21:e5f0f5abb5ae, committed 2020-10-25

Comitter:

Nanaud

Date:

Sun Oct 25 22:36:51 2020 +0000

Parent:

20:7d206773f39e

Child:

22:f891c2bce091

Commit message:

J-4

Changed in this revision

--- a/captUS.cpp	Tue Oct 20 17:53:32 2020 +0000
+++ b/captUS.cpp	Sun Oct 25 22:36:51 2020 +0000
@@ -1,10 +1,14 @@
 //Nom du fichier : captUS.cpp
 #include "pins.h"
-const float DISTLIM = 160;
+const float DISTLIM = 450; //160
 
 // Variables globales & timers
-float us_out[6];
-float* distance;
+double us_high[6]={0};
+double us_low[6]={0};
+double us_diff[6]={0};
+bool us_verif[6]={0};
+
+double distt[6];
 Timer tps;
 Ticker ticker_US;
 bool rebooted = 0;
@@ -12,69 +16,63 @@
 
 //int sptt = 0;
 
+/*
 void captUS_init()
 {
-    ::distance = new float(6); //équivalent au malloc()
+    ::distance = new double(6); //équivalent au malloc()
     tps.reset();
     tps.start();
 }
+*/
 
 void captUS_trig()
 {
     convertToDistance();
 
     if((objRecule[indice]==0) && (fnc == 2)) {
-        if ((::distance[1] >= DISTLIM) && (::distance[0] >= DISTLIM)  && (::distance[5] >= DISTLIM)) {
-            /*
-            if (rebooted == 1) {
-                cmdG = 0;
-                cmdD = 0;
-                rebooted = 0;
-            }
-            */
-            //mot_en();
+        if ((distt[5] >= DISTLIM) && (distt[0] >= DISTLIM)  && (distt[1] >= DISTLIM)) {
             wtt = 1;
-            //sptt=0;
-
-        } else {
-            //sptt++;
+        }
 
-            //if(sptt>=5) {
+        else {
             mot_dis();
-            //stt=0;
-            //acc = 1;
             rebooted = 1;
             wtt=0;
-            //}
         }
     }
 
     else if((objRecule[indice]==1) && (fnc == 2)) {
-        if ((::distance[2] >= DISTLIM) && (::distance[3] >= DISTLIM) && (::distance[4] >= DISTLIM)) {
-            /*
-            if (rebooted == 1) {
-                cmdG = 0;
-                cmdD = 0;
-                rebooted = 0;
-            }
-            */
-            //mot_en();
+        if ((distt[2] >= DISTLIM) && (distt[3] >= DISTLIM) && (distt[4] >= DISTLIM)) {
             wtt=1;
-            //sptt=0;
-
-        } else {
-            //sptt++;
+        }
 
-            //if(sptt>=5) {
+        else {
             mot_dis();
-            //stt=0;
-            //acc = 1;
             rebooted = 1;
             wtt=0;
-            //}
         }
     }
 
+    /*
+    pc.printf("US1 = %5.0lf uS\n\r", us_out[0]);
+    pc.printf("US2 = %5.0lf uS\n\r", us_out[1]);
+    //pc.printf("US3 = %5.0lf uS\n\r", us_out[2]);
+    //pc.printf("US4 = %5.0lf uS\n\r", us_out[3]);
+    //pc.printf("US5 = %5.0lf uS\n\r", us_out[4]);
+    pc.printf("US6 = %5.0lf uS\n\r", us_out[5]);
+    pc.printf("\n\r");
+    */
+
+    /*
+    pc.printf("Dist1 = %5.0lf mm\n\r", ::distance[0]);
+    pc.printf("Dist2 = %5.0lf mm\n\r", ::distance[1]);
+    pc.printf("Dist3 = %5.0lf mm\n\r", ::distance[2]);
+    pc.printf("Dist4 = %5.0lf mm\n\r", ::distance[3]);
+    pc.printf("Dist5 = %5.0lf mm\n\r", ::distance[4]);
+    pc.printf("Dist6 = %5.0lf mm\n\r", ::distance[5]);
+    pc.printf("\n\r");
+    */
+
     tps.reset();
     trigger=1;
     wait(0.00002);
@@ -83,60 +81,108 @@
 
 void echoRise1()
 {
-    us_out[0]=tps.read_us();
+    if(us_verif[0] == 0) {
+        us_high[0]=tps.read_us();
+        us_verif[0] = 1;
+    }
 }
+
 void echoFall1()
 {
-    us_out[0]=(tps.read_us()-us_out[0])/2;
+    if(us_verif[0] == 1) {
+        us_low[0]=tps.read_us();
+        us_diff[0]=us_low[0]-us_high[0];
+        us_verif[0] = 0;
+    }
 }
 
 void echoRise2()
 {
-    us_out[1]=tps.read_us();
+    if(us_verif[1] == 0) {
+        us_high[1]=tps.read_us();
+        us_verif[1] = 1;
+    }
 }
+
 void echoFall2()
 {
-    us_out[1]=(tps.read_us()-us_out[1])/2;
+    if(us_verif[1] == 1) {
+        us_low[1]=tps.read_us();
+        us_diff[1]=us_low[1]-us_high[1];
+        us_verif[1] = 0;
+    }
 }
 
 void echoRise3()
 {
-    us_out[2]=tps.read_us();
+    if(us_verif[2] == 0) {
+        us_high[2]=tps.read_us();
+        us_verif[2] = 1;
+    }
 }
+
 void echoFall3()
 {
-    us_out[2]=(tps.read_us()-us_out[2])/2;
+    if(us_verif[2] == 1) {
+        us_low[2]=tps.read_us();
+        us_diff[2]=us_low[2]-us_high[2];
+        us_verif[2] = 0;
+    }
 }
 
 void echoRise4()
 {
-    us_out[3]=tps.read_us();
+    if(us_verif[3] == 0) {
+        us_high[3]=tps.read_us();
+        us_verif[3] = 1;
+    }
 }
+
 void echoFall4()
 {
-    us_out[3]=(tps.read_us()-us_out[3])/2;
+    if(us_verif[3] == 1) {
+        us_low[3]=tps.read_us();
+        us_diff[3]=us_low[3]-us_high[3];
+        us_verif[3] = 0;
+    }
 }
 
 void echoRise5()
 {
-    us_out[4]=tps.read_us();
+    if(us_verif[4] == 0) {
+        us_high[4]=tps.read_us();
+        us_verif[4] = 1;
+    }
 }
+
 void echoFall5()
 {
-    us_out[4]=(tps.read_us()-us_out[4])/2;
+    if(us_verif[4] == 1) {
+        us_low[4]=tps.read_us();
+        us_diff[4]=us_low[4]-us_high[4];
+        us_verif[4] = 0;
+    }
 }
 
 void echoRise6()
 {
-    us_out[5]=tps.read_us();
+    if(us_verif[5] == 0) {
+        us_high[5]=tps.read_us();
+        us_verif[5] = 1;
+    }
 }
+
 void echoFall6()
 {
-    us_out[5]=(tps.read_us()-us_out[5])/2;
+    if(us_verif[5] == 1) {
+        us_low[5]=tps.read_us();
+        us_diff[5]=us_low[5]-us_high[5];
+        us_verif[5] = 0;
+    }
 }
 
 
-float* convertToDistance()
+void convertToDistance()
 {
     /**************************************
      * Nous convertisons grâce au valeur  *
@@ -144,50 +190,12 @@
      * et echoFallx                       *
      **************************************/
 
-    for(char i = 0; i<6; i++) {
-        ::distance[i] = (us_out[i]*340)/1000;//conversion en distance(mm)
+    for(int i = 0; i<6; i++) {
+        distt[i] = ((us_diff[i]*340)/1000); //conversion en distance(mm)
     }
 
     /****************************************
      * nous retournons l'adresse du tableau *
      ****************************************/
-    return ::distance;
+    //return ::distance;
 }
-
-bool obstacleSpoted(float dist,double x_robot,double y_robot,double phi, char I_theta)
-{
-    /**************************
-     * convertion de ° en rad *
-     **************************/
-    double phiG = ((phi+(I_theta*THETA))*_PI_)/180;
-
-    /***********************************
-     * convertion de la norme grâce à  *
-     * la norme et à l'angle           *
-     ***********************************/
-    double x_dect = dist * cos(phiG) + x_robot;
-    double y_dect = dist * sin(phiG) + y_robot;
-
-
-    changementBase(&x_dect,&y_dect);
-    /*********************************************
-     * vérification de la position de l'obstacle *
-     *********************************************/
-    if(x_dect > LARGEUR_TAB || x_dect < 0||y_dect >LONGUEUR_TAB||y_dect < 0) {
-        return false;
-    } else {
-        return true;
-    }
-
-}
-
-void changementBase(double* x_detect, double* y_detect)
-{
-    /*****************************
-     * l'origine de notres table *
-     * se situe au coins         *
-     * supérieur                 *
-     *****************************/
-    *y_detect+=505;
-    *x_detect+=5;
-}

--- a/captUS.h	Tue Oct 20 17:53:32 2020 +0000
+++ b/captUS.h	Sun Oct 25 22:36:51 2020 +0000
@@ -1,7 +1,7 @@
 // Nom du fichier : captUS.h
 
-#ifndef CAPTUS_H
-#define CAPTUS_H
+//#ifndef CAPTUS_H
+//#define CAPTUS_H
 //#include "mbed.h"
 #include "math.h"
 
@@ -11,17 +11,23 @@
 #define THETA 60
 
 // extern
-extern float us_out[6];
+//extern double us_out[6];
+
+extern double us_high[6];
+extern double us_low[6];
+extern double us_diff[6];
+extern bool us_verif[6];
+
 extern Timer tps;
 extern Ticker ticker_US;
-extern float* distance;
+extern double distt[6];
 
 extern bool rebooted;
 extern bool wtt;
 
 // Prototypes
 void captUS_trig();
-void captUS_init();
+//void captUS_init();
 
 void echoRise1();
 void echoFall1();
@@ -40,23 +46,23 @@
  * Création d'une fonction qui    *
  * convertis le temps en distance *
  **********************************/
-float* convertToDistance();
+void convertToDistance(void);
  
  
 /********************************************
  * nous permet de placer l'origine au coins *
  * de la table                              *
  ********************************************/
-void changementBase(double* x_detect, double* y_detect);
+//void changementBase(double* x_detect, double* y_detect);
  
 /*******************************************
  * nous permet de détecter un obstacle     *
  *                                         *
  * false : personne | true : quelquechoses *
  *******************************************/
-bool obstacleSpoted(float dist,double x_robot,double y_robot ,double phi, char I_theta);
+//bool obstacleSpoted(float dist,double x_robot,double y_robot ,double phi, char I_theta);
  
  
  
  
-#endif // CAPTUS_H
\ No newline at end of file
+//#endif // CAPTUS_H
\ No newline at end of file

--- a/debugPC.cpp	Tue Oct 20 17:53:32 2020 +0000
+++ b/debugPC.cpp	Sun Oct 25 22:36:51 2020 +0000
@@ -305,9 +305,9 @@
             //bt.printf("O = %lf\n\r", (O*180)/_PI_);
             //bt.printf("vitG = %lf\n\r", vitG);
             //bt.printf("vitD = %lf\n\r", vitD);
-            bt.printf("dist 1 = %f\n\r", ::distance[0]);
-            bt.printf("dist 2 = %f\n\r", ::distance[1]);
-            bt.printf("dist 6 = %f\n\r", ::distance[5]);
+            //bt.printf("dist 1 = %f\n\r", ::distance[0]);
+            //bt.printf("dist 2 = %f\n\r", ::distance[1]);
+            //bt.printf("dist 6 = %f\n\r", ::distance[5]);
             bt.printf("fnc = %d\n\r", fnc);
             break;
         default:
@@ -384,40 +384,42 @@
 
 void affUltrasons()
 {
-    /*
     // Données bruts
 
+    /*
     // pc
-    if(aff_US[0]) pc.printf("Tps US1 = %5.0f uS\n\r", us_out[0]);
-    if(aff_US[1]) pc.printf("Tps US2 = %5.0f uS\n\r", us_out[1]);
-    if(aff_US[2]) pc.printf("Tps US3 = %5.0f uS\n\r", us_out[2]);
-    if(aff_US[3]) pc.printf("Tps US4 = %5.0f uS\n\r", us_out[3]);
-    if(aff_US[4]) pc.printf("Tps US5 = %5.0f uS\n\r", us_out[4]);
-    if(aff_US[5]) pc.printf("Tps US6 = %5.0f uS\n\r", us_out[5]);
+    if(aff_US[0]) pc.printf("Tps US1 = %5.0f uS\n\r", us_diff[0]);
+    if(aff_US[1]) pc.printf("Tps US2 = %5.0f uS\n\r", us_diff[1]);
+    if(aff_US[2]) pc.printf("Tps US3 = %5.0f uS\n\r", us_diff[2]);
+    if(aff_US[3]) pc.printf("Tps US4 = %5.0f uS\n\r", us_diff[3]);
+    if(aff_US[4]) pc.printf("Tps US5 = %5.0f uS\n\r", us_diff[4]);
+    if(aff_US[5]) pc.printf("Tps US6 = %5.0f uS\n\r", us_diff[5]);
     if(aff_US[0]||aff_US[1]||aff_US[2]||aff_US[3]||aff_US[4]||aff_US[5]) pc.printf("\n\r");
 
     // bt
-    if(aff_US[0]) bt.printf("Tps US1 = %5.0f uS\n\r", us_out[0]);
-    if(aff_US[1]) bt.printf("Tps US2 = %5.0f uS\n\r", us_out[1]);
-    if(aff_US[2]) bt.printf("Tps US3 = %5.0f uS\n\r", us_out[2]);
-    if(aff_US[3]) bt.printf("Tps US4 = %5.0f uS\n\r", us_out[3]);
-    if(aff_US[4]) bt.printf("Tps US5 = %5.0f uS\n\r", us_out[4]);
-    if(aff_US[5]) bt.printf("Tps US6 = %5.0f uS\n\r", us_out[5]);
+    if(aff_US[0]) bt.printf("Tps US1 = %5.0f uS\n\r", us_diff[0]);
+    if(aff_US[1]) bt.printf("Tps US2 = %5.0f uS\n\r", us_diff[1]);
+    if(aff_US[2]) bt.printf("Tps US3 = %5.0f uS\n\r", us_diff[2]);
+    if(aff_US[3]) bt.printf("Tps US4 = %5.0f uS\n\r", us_diff[3]);
+    if(aff_US[4]) bt.printf("Tps US5 = %5.0f uS\n\r", us_diff[4]);
+    if(aff_US[5]) bt.printf("Tps US6 = %5.0f uS\n\r", us_diff[5]);
     if(aff_US[0]||aff_US[1]||aff_US[2]||aff_US[3]||aff_US[4]||aff_US[5]) bt.printf("\n\r");
     */
 
+    
     // Distances
 
     // pc
-    pc.printf("distance[0] = %f\n\r", ::distance[0]);
-    //pc.printf("distance[1] = %f\n\r", ::distance[1]);
-    //pc.printf("distance[2] = %f\n\r", ::distance[2]);
-    //pc.printf("distance[3] = %f\n\r", ::distance[3]);
-    //pc.printf("distance[4] = %f\n\r", ::distance[4]);
-    //pc.printf("distance[5] = %f\n\r", ::distance[5]);
+    pc.printf("distance[0] = %lf\n\r", distt[0]);
+    pc.printf("distance[1] = %lf\n\r", distt[1]);
+    pc.printf("distance[2] = %lf\n\r", distt[2]);
+    pc.printf("distance[3] = %lf\n\r", distt[3]);
+    pc.printf("distance[4] = %lf\n\r", distt[4]);
+    pc.printf("distance[5] = %lf\n\r", distt[5]);
     pc.printf("\n\r");
 
     // bt
+
     /*
     bt.printf("distance[0] = %f\n\r", ::distance[0]);
     bt.printf("distance[1] = %f\n\r", ::distance[1]);

--- a/main.cpp	Tue Oct 20 17:53:32 2020 +0000
+++ b/main.cpp	Sun Oct 25 22:36:51 2020 +0000
@@ -27,7 +27,7 @@
 {
     cptGlobal++;
     
-    if(cptGlobal==95) {
+    if(cptGlobal==5) { //95
         FlagGOTO(0);
     }
     
@@ -51,7 +51,8 @@
 
 int main()
 {
-    captUS_init();
+    //captUS_init();
+    tps.start();
     FlagGOTO(90);
     TimerGlobal.attach(&fctCptGlobal, 1.0);
     
@@ -71,7 +72,7 @@
     bt.baud(9600);
     bt.format(8,SerialBase::None,1);
 
-    ticker_US.attach(&captUS_trig,0.2); // On apelle cette fonction toutes 0.2 secondes
+    ticker_US.attach(&captUS_trig,0.1); // 0.2
     //ticker_affUS.attach(&affUltrasons,1.0);
     //ticker_affcd.attach(&affCodeurs,1.0);
     //ticker_odo.attach(&odo2,0.02);
@@ -79,7 +80,7 @@
     //ticker_affodo.attach(&affOdo,1.0);
 
     // Init capteurs à ultrasons
-    captUS_init();
+    //captUS_init();
     echo1.rise(&echoRise1);
     echo1.fall(&echoFall1);
     echo2.rise(&echoRise2);