Diff: captUS.cpp

Revision:

16:4c0b1647e8ae

Parent:

15:43f5bda97488

--- a/captUS.cpp	Mon Sep 28 19:17:00 2020 +0000
+++ b/captUS.cpp	Tue Oct 13 14:50:31 2020 +0000
@@ -1,16 +1,22 @@
+#include "captUS.h"
+#include "pins.h"
+#include "math.h"
 //Nom du fichier : captUS.cpp
-#include "pins.h"
 
 // Variables globales & timers
 float us_out[6];
+float* distance;
 Timer tps;
 Ticker ticker_US;
 
+
 void captUS_init(){
+    ::distance = new float(6); //équivalent au malloc()
     tps.reset();
     tps.start();
 }
 
+
 void captUS_trig(){
     tps.reset();
     trigger=1;
@@ -39,22 +45,56 @@
 
 float* convertToDistance(){
     /**************************************
-     *création d'un tableau où l'ensemble *
-     *des distance serons stockées        *
-     **************************************/
-    float distance[6];
-    
-    /**************************************
      * Nous convertisons grâce au valeur  *
      * qui sont retournées par echoRiseX  *
      * et echoFallx                       *
      **************************************/
-    for(char i = 0; i<6;i++)
-        distance[i] = 10*(us_out[i].read_us()-correction)/58.0 ;
+    
+    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;
+
+    /***********************************
+     * 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;
+    
     
-    /*************************************
-     * Nous retournons le tableau qui    *
-     * contiens l'ensemble des distances *
-     *************************************/ 
-    return distance;    
+    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;
+}
+
+