Diff: captUS.cpp

Revision:

20:7d206773f39e

Parent:

1:2fe8c402ee79

Child:

21:e5f0f5abb5ae

--- a/captUS.cpp	Mon Oct 12 19:17:40 2020 +0000
+++ b/captUS.cpp	Tue Oct 20 17:53:32 2020 +0000
@@ -1,35 +1,193 @@
 //Nom du fichier : captUS.cpp
 #include "pins.h"
+const float DISTLIM = 160;
 
 // Variables globales & timers
 float us_out[6];
+float* distance;
 Timer tps;
 Ticker ticker_US;
+bool rebooted = 0;
+bool wtt = 0;
 
-void captUS_init(){
-    tps.start();}
+//int sptt = 0;
+
+void captUS_init()
+{
+    ::distance = new float(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();
+            wtt = 1;
+            //sptt=0;
+
+        } else {
+            //sptt++;
 
-void captUS_trig(){
+            //if(sptt>=5) {
+            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();
+            wtt=1;
+            //sptt=0;
+
+        } else {
+            //sptt++;
+
+            //if(sptt>=5) {
+            mot_dis();
+            //stt=0;
+            //acc = 1;
+            rebooted = 1;
+            wtt=0;
+            //}
+        }
+    }
+
     tps.reset();
     trigger=1;
     wait(0.00002);
     trigger=0;
 }
 
-void echoRise1(){us_out[0]=tps.read_us();}
-void echoFall1(){us_out[0]=(tps.read_us()-us_out[0])/2;}
+void echoRise1()
+{
+    us_out[0]=tps.read_us();
+}
+void echoFall1()
+{
+    us_out[0]=(tps.read_us()-us_out[0])/2;
+}
+
+void echoRise2()
+{
+    us_out[1]=tps.read_us();
+}
+void echoFall2()
+{
+    us_out[1]=(tps.read_us()-us_out[1])/2;
+}
+
+void echoRise3()
+{
+    us_out[2]=tps.read_us();
+}
+void echoFall3()
+{
+    us_out[2]=(tps.read_us()-us_out[2])/2;
+}
 
-void echoRise2(){us_out[1]=tps.read_us();}
-void echoFall2(){us_out[1]=(tps.read_us()-us_out[1])/2;}
+void echoRise4()
+{
+    us_out[3]=tps.read_us();
+}
+void echoFall4()
+{
+    us_out[3]=(tps.read_us()-us_out[3])/2;
+}
 
-void echoRise3(){us_out[2]=tps.read_us();}
-void echoFall3(){us_out[2]=(tps.read_us()-us_out[2])/2;}
+void echoRise5()
+{
+    us_out[4]=tps.read_us();
+}
+void echoFall5()
+{
+    us_out[4]=(tps.read_us()-us_out[4])/2;
+}
+
+void echoRise6()
+{
+    us_out[5]=tps.read_us();
+}
+void echoFall6()
+{
+    us_out[5]=(tps.read_us()-us_out[5])/2;
+}
+
 
-void echoRise4(){us_out[3]=tps.read_us();}
-void echoFall4(){us_out[3]=(tps.read_us()-us_out[3])/2;}
+float* convertToDistance()
+{
+    /**************************************
+     * Nous convertisons grâce au valeur  *
+     * qui sont retournées par echoRiseX  *
+     * et echoFallx                       *
+     **************************************/
+
+    for(char i = 0; i<6; i++) {
+        ::distance[i] = (us_out[i]*340)/1000;//conversion en distance(mm)
+    }
+
+    /****************************************
+     * nous retournons l'adresse du tableau *
+     ****************************************/
+    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;
 
-void echoRise5(){us_out[4]=tps.read_us();}
-void echoFall5(){us_out[4]=(tps.read_us()-us_out[4])/2;}
+    /***********************************
+     * 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;
+
 
-void echoRise6(){us_out[5]=tps.read_us();}
-void echoFall6(){us_out[5]=(tps.read_us()-us_out[5])/2;}
+    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;
+}