AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode

Dependencies:   mbed DRV8825

Revision:
25:869b1c1f51a7
Parent:
24:be2b2be6907b
Child:
26:bb2b778bd351
--- a/captUS.cpp	Wed Oct 28 20:08:39 2020 +0000
+++ b/captUS.cpp	Thu Oct 29 07:53:25 2020 +0000
@@ -2,130 +2,77 @@
 #include "pins.h"
 
 /* #define */
-#define DISTLIM 600
-#define MAXMOY 3
+#define DISTLIM 600 // Distance max de détection
+#define OFFSET 200
+#define MAXMOY 3 // Nombre de mesure pour la moyenne
 
 /* Variables globales */
-Timer TimUS;
-Ticker TickUS_actu;
-
-//const float DISTLIM = 600;
+Timer TimUS; // Timer pour la mesure de distance entre le robot
+Ticker TickUS_actu; // Actualisation valeur distance détection d'obstacle
 
-unsigned int us_high[6]= {0};
-unsigned int us_low[6]= {0};
-unsigned int us_diff[6]= {0};
-bool us_verif[6]= {0};
+unsigned int us_high[6] = {0}; // Valeurs du timer lorsqu'il y a une interruption sur front montant
+unsigned int us_low[6] = {0}; // Valeurs du timer lorsqu'il y a une interruption sur front descendant
+unsigned int us_diff[6] = {0}; // Différence entre ces deux temps
+//bool us_verif[6] = {0}; // Permet d'enchainer dans le bon ordre : front montant puis front descendant
+bool us_verif2[2][6] = {0};
+
+unsigned int us_dist[6] = {0}; // Valeurs des distances
+bool us_rbt_restart = 0; // Le robot a détecté un obstacle et s'arrête
+bool us_libre = 0; // Le robot ne détecte plus d'obstacle, il est prêt à redémarrer
 
-unsigned int distt[6];
-bool rebooted = 0;
-bool wtt = 0;
-
-int indix = 0;
-unsigned int total[6] = {0};
-unsigned int ttt[6] = {0};
+int i = 0;
+unsigned int us_dist_total[6] = {0};
+unsigned int us_dist_moy[6] = {0};
 
-void calculTotal()
+void captUS_moyenne()
 {
-    if(indix < MAXMOY) {
+    if(i < MAXMOY) {
         for(int j=0; j<6 ; j++) {
-            total[j]+=distt[j];
+            us_dist_total[j] += us_dist[j];
         }
-        indix++;
+        i++;
+
     } else {
         for(int j=0; j<6 ; j++) {
-            ttt[j]=total[j]/MAXMOY;
-            total[j]=0;
+            us_dist_moy[j]=us_dist_total[j] / MAXMOY;
+            us_dist_total[j]=0;
         }
 
-        /*
-        pc.printf("ttt[1] = %d\n\r", ttt[0]);
-        pc.printf("ttt[2] = %d\n\r", ttt[1]);
-        pc.printf("ttt[3] = %d\n\r", ttt[2]);
-        pc.printf("ttt[4] = %d\n\r", ttt[3]);
-        pc.printf("ttt[5] = %d\n\r", ttt[4]);
-        pc.printf("ttt[6] = %d\n\r", ttt[5]);
-        pc.printf("\n\r");
-        */
-
-        indix=0;
+        i=0;
     }
 }
 
-/*
-void captUS_init()
-{
-    ::distance = new double(6); //équivalent au malloc()
-    tps.reset();
-    tps.start();
-}
-*/
 
 void captUS_trig()
 {
-    convertToDistance();
+    captUS_convToDist();
 
-    calculTotal();
+    captUS_moyenne();
 
-    if((objRecule[indice]==0) && (fnc == 2)) {
-        //if ((distt[5] >= DISTLIM) && (distt[0] >= DISTLIM)  && (distt[1] >= DISTLIM)) {
-        //if (((distt[5] >= DISTLIM) && (distt[0] >= DISTLIM)  && (distt[1] >= DISTLIM)) /*|| ((distt[5] < 0) && (distt[0] < 0)  && (distt[1] < 0))*/) {
-        if ((ttt[5] >= DISTLIM) && (ttt[0] >= DISTLIM)  && (ttt[1] >= DISTLIM)) {
-            wtt = 1;
+    if((objRecule[indiceStrategie]==0) && (action == 2)) {
+        if ((us_dist_moy[5] >= (DISTLIM-OFFSET)) && (us_dist_moy[0] >= DISTLIM)  && (us_dist_moy[1] >= (DISTLIM-OFFSET))) {
+            us_libre = 1;
         }
 
         else {
             mot_dis();
-            rebooted = 1;
-            wtt=0;
+            us_rbt_restart = 1;
+            us_libre=0;
         }
     }
 
-    else if((objRecule[indice]==1) && (fnc == 2)) {
-        //if ((distt[2] >= DISTLIM) && (distt[3] >= DISTLIM) && (distt[4] >= DISTLIM)) {
-        //if (((distt[2] >= DISTLIM) && (distt[3] >= DISTLIM)  && (distt[4] >= DISTLIM)) /*|| ((distt[2] < 0) && (distt[3] < 0)  && (distt[4] < 0))*/) {
-        if ((ttt[2] >= DISTLIM) && (ttt[3] >= DISTLIM)  && (ttt[4] >= DISTLIM)) {
-            wtt=1;
+    else if((objRecule[indiceStrategie]==1) && (action == 2)) {
+        if ((us_dist_moy[2] >= (DISTLIM-OFFSET)) && (us_dist_moy[3] >= DISTLIM)  && (us_dist_moy[4] >= (DISTLIM-OFFSET))) {
+            us_libre=1;
         }
 
         else {
             mot_dis();
-            rebooted = 1;
-            wtt=0;
+            us_rbt_restart = 1;
+            us_libre=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");
-    */
-    
-    /*
-    pc.printf("ttt[1] = %d\n\r", ttt[0]);
-    pc.printf("ttt[2] = %d\n\r", ttt[1]);
-    pc.printf("ttt[3] = %d\n\r", ttt[2]);
-    pc.printf("ttt[4] = %d\n\r", ttt[3]);
-    pc.printf("ttt[5] = %d\n\r", ttt[4]);
-    pc.printf("ttt[6] = %d\n\r", ttt[5]);
-    pc.printf("\n\r");
-    */
-
-    //calculTotal();
-
     TimUS.reset();
     trigger=1;
     wait(0.00002);
@@ -134,108 +81,102 @@
 
 void echoRise1()
 {
-    if(us_verif[0] == 0) {
+    if(us_verif2[1][0] == false) {
         us_high[0]=TimUS.read_us();
-        us_verif[0] = 1;
+        us_verif2[0][0] = true;
     }
 }
 
 void echoFall1()
 {
-    if(us_verif[0] == 1) {
+    if(us_verif2[0][0] == true) {
         us_low[0]=TimUS.read_us();
-        us_diff[0]=us_low[0]-us_high[0];
-        us_verif[0] = 0;
+        us_verif2[1][0] = true;
     }
 }
 
 void echoRise2()
 {
-    if(us_verif[1] == 0) {
+    if(us_verif2[1][1] == false) {
         us_high[1]=TimUS.read_us();
-        us_verif[1] = 1;
+        us_verif2[0][1] = true;
     }
 }
 
 void echoFall2()
 {
-    if(us_verif[1] == 1) {
+    if(us_verif2[0][1] == true) {
         us_low[1]=TimUS.read_us();
-        us_diff[1]=us_low[1]-us_high[1];
-        us_verif[1] = 0;
+        us_verif2[1][1] = true;
     }
 }
 
 void echoRise3()
 {
-    if(us_verif[2] == 0) {
+    if(us_verif2[1][2] == false) {
         us_high[2]=TimUS.read_us();
-        us_verif[2] = 1;
+        us_verif2[0][2] = true;
     }
 }
 
 void echoFall3()
 {
-    if(us_verif[2] == 1) {
+    if(us_verif2[0][2] == true) {
         us_low[2]=TimUS.read_us();
-        us_diff[2]=us_low[2]-us_high[2];
-        us_verif[2] = 0;
+        us_verif2[1][2] = true;
     }
 }
 
 void echoRise4()
 {
-    if(us_verif[3] == 0) {
+    if(us_verif2[1][3] == false) {
         us_high[3]=TimUS.read_us();
-        us_verif[3] = 1;
+        us_verif2[0][3] = true;
     }
 }
 
 void echoFall4()
 {
-    if(us_verif[3] == 1) {
+    if(us_verif2[0][3] == true) {
         us_low[3]=TimUS.read_us();
-        us_diff[3]=us_low[3]-us_high[3];
-        us_verif[3] = 0;
+        us_verif2[1][3] = true;
     }
 }
 
 void echoRise5()
 {
-    if(us_verif[4] == 0) {
+    if(us_verif2[1][4] == false) {
         us_high[4]=TimUS.read_us();
-        us_verif[4] = 1;
+        us_verif2[0][4] = true;
     }
 }
 
 void echoFall5()
 {
-    if(us_verif[4] == 1) {
+    if(us_verif2[0][4] == true) {
         us_low[4]=TimUS.read_us();
-        us_diff[4]=us_low[4]-us_high[4];
-        us_verif[4] = 0;
+        us_verif2[1][4] = true;
     }
 }
 
 void echoRise6()
 {
-    if(us_verif[5] == 0) {
+    if(us_verif2[1][5] == false) {
         us_high[5]=TimUS.read_us();
-        us_verif[5] = 1;
+        us_verif2[0][5] = true;
     }
 }
 
 void echoFall6()
 {
-    if(us_verif[5] == 1) {
+    if(us_verif2[0][5] == true) {
         us_low[5]=TimUS.read_us();
-        us_diff[5]=us_low[5]-us_high[5];
-        us_verif[5] = 0;
+        us_verif2[1][5] = true;
     }
 }
 
 
-void convertToDistance()
+void captUS_convToDist()
 {
     /**************************************
      * Nous convertisons grâce au valeur  *
@@ -244,11 +185,9 @@
      **************************************/
 
     for(int i = 0; i<6; i++) {
-        distt[i] = (int) ((us_diff[i]*340)/1000); //conversion en distance(mm)
+        if(us_verif2[0][i] == true && us_verif2[1][i] == true)
+            us_dist[i] = (int) (((us_low[i] - us_high[i])*340)/1000); //conversion en distance(mm)
+        else
+            us_dist[i] = LARGEUR_TAB;
     }
-
-    /****************************************
-     * nous retournons l'adresse du tableau *
-     ****************************************/
-    //return ::distance;
 }