Gaëtan Andrieu / Mbed 2 deprecated I2Cboussole_avec_librairie

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed vl53l0x_api Motordriver HCSR05 DWM_API CMPS03_PWM PID_boussole_ Servolib

Files at this revision

API Documentation at this revision

Revision 2:25abeda178d4, committed 2019-04-03

Comitter:
Gaetan_Andrieu
Date:
Wed Apr 03 22:49:33 2019 +0000
Parent:
1:e3c0b389788e
Commit message:
Commander robot dans differentes pieces via le bluetooth

Changed in this revision

CMPS03_PWM.lib Show annotated file Show diff for this revision Revisions of this file
CPMS03_I2C.lib Show diff for this revision Revisions of this file
HCSR05.lib Show annotated file Show diff for this revision Revisions of this file
PID_boussole.lib Show annotated file Show diff for this revision Revisions of this file
Servolib.lib Show annotated file Show diff for this revision Revisions of this file
capteur_laser.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
vl53l0x_api.lib Show annotated file Show diff for this revision Revisions of this file 
--- a/CMPS03_PWM.lib	Mon Apr 01 16:19:27 2019 +0000
+++ b/CMPS03_PWM.lib	Wed Apr 03 22:49:33 2019 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/Gaetan_Andrieu/code/CMPS03_PWM/#dd04e215dd63
+https://os.mbed.com/users/Gaetan_Andrieu/code/CMPS03_PWM/#cb5d4002be15

--- a/CPMS03_I2C.lib	Mon Apr 01 16:19:27 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/users/Gaetony/code/CPMS03_I2C/#7f3d329d0b8f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HCSR05.lib	Wed Apr 03 22:49:33 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Gaetan_Andrieu/code/HCSR05/#fc3ae06560ef

--- a/PID_boussole.lib	Mon Apr 01 16:19:27 2019 +0000
+++ b/PID_boussole.lib	Wed Apr 03 22:49:33 2019 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/Gaetan_Andrieu/code/PID_boussole/#4e5a5b017550
+https://os.mbed.com/users/Gaetan_Andrieu/code/PID_boussole_/#1d8f9c73cdc4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servolib.lib	Wed Apr 03 22:49:33 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/TABLE4/code/Servolib/#2f33e8615692

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/capteur_laser.h	Wed Apr 03 22:49:33 2019 +0000
@@ -0,0 +1,100 @@
+#include "vl53l0x_api.h"
+#include "vl53l0x_platform.h"
+#include "vl53l0x_i2c_platform.h"
+
+#define USE_I2C_2V8
+
+VL53L0X_Error WaitMeasurementDataReady(VL53L0X_DEV Dev) {
+    VL53L0X_Error Status = VL53L0X_ERROR_NONE;
+    uint8_t NewDatReady=0;
+    uint32_t LoopNb;
+    
+    if (Status == VL53L0X_ERROR_NONE) {
+        LoopNb = 0;
+        do {
+            Status = VL53L0X_GetMeasurementDataReady(Dev, &NewDatReady);
+            if ((NewDatReady == 0x01) || Status != VL53L0X_ERROR_NONE) {
+                break;
+            }
+            LoopNb = LoopNb + 1;
+            VL53L0X_PollingDelay(Dev);
+        } while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP);
+
+        if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
+            Status = VL53L0X_ERROR_TIME_OUT;
+        }
+    }
+
+    return Status;
+}
+
+VL53L0X_Error WaitStopCompleted(VL53L0X_DEV Dev) {
+    VL53L0X_Error Status = VL53L0X_ERROR_NONE;
+    uint32_t StopCompleted=0;
+    uint32_t LoopNb;
+
+    if (Status == VL53L0X_ERROR_NONE) {
+        LoopNb = 0;
+        do {
+            Status = VL53L0X_GetStopCompletedStatus(Dev, &StopCompleted);
+            if ((StopCompleted == 0x00) || Status != VL53L0X_ERROR_NONE) {
+                break;
+            }
+            LoopNb = LoopNb + 1;
+            VL53L0X_PollingDelay(Dev);
+        } while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP);
+
+        if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
+            Status = VL53L0X_ERROR_TIME_OUT;
+        }
+
+    }
+
+    return Status;
+}
+
+VL53L0X_Dev_t MyDevice;
+VL53L0X_Dev_t *pMyDevice = &MyDevice;
+VL53L0X_RangingMeasurementData_t    RangingMeasurementData;
+VL53L0X_RangingMeasurementData_t   *pRangingMeasurementData    = &RangingMeasurementData;
+    
+void laser_init()
+{
+    // Initialize Comms
+    pMyDevice->I2cDevAddr      = 0x52;
+    pMyDevice->comms_type      =  1;
+    pMyDevice->comms_speed_khz =  400;
+        
+        
+    //VL53L0X_ERROR_CONTROL_INTERFACE;
+    VL53L0X_RdWord(&MyDevice, VL53L0X_REG_OSC_CALIBRATE_VAL,0);
+    VL53L0X_DataInit(&MyDevice); // Data initialization
+    //Status = VL53L0X_ERROR_NONE;
+    uint32_t refSpadCount;
+    uint8_t isApertureSpads;
+    uint8_t VhvSettings;
+    uint8_t PhaseCal;
+    
+    VL53L0X_StaticInit(pMyDevice); // Device Initialization
+    VL53L0X_PerformRefSpadManagement(pMyDevice, &refSpadCount, &isApertureSpads); // Device Initialization
+    VL53L0X_PerformRefCalibration(pMyDevice, &VhvSettings, &PhaseCal); // Device Initialization
+    VL53L0X_SetDeviceMode(pMyDevice, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in single ranging mode
+    VL53L0X_StartMeasurement(pMyDevice);
+}
+
+int laser_mesure(int nb_mesure)
+{
+    int measure=0;
+    int sum=0;
+    for(int i=0; i<nb_mesure; i++){
+        WaitMeasurementDataReady(pMyDevice);
+        VL53L0X_GetRangingMeasurementData(pMyDevice, pRangingMeasurementData);
+        measure=pRangingMeasurementData->RangeMilliMeter;
+        //printf("In loop measurement %d\n", mea);
+        sum=sum+measure;
+        // Clear the interrupt
+        VL53L0X_ClearInterruptMask(pMyDevice, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
+        VL53L0X_PollingDelay(pMyDevice);
+    }
+    return sum/nb_mesure;
+}
\ No newline at end of file

--- a/main.cpp	Mon Apr 01 16:19:27 2019 +0000
+++ b/main.cpp	Wed Apr 03 22:49:33 2019 +0000
@@ -1,23 +1,229 @@
 #include "mbed.h"
 #include "CMPS03.h"
+#include "HCSR05.h"
 #include "PIDboussole.h"
 #include "dwm_uart.h"
+#include "capteur_laser.h"
+#include "Servo.h"
+
 #define PI 3.14159265
+#define safe_distance 200
+#define GAIN_CONTOURNER 0.5
+#define ALPHA0 20
+#define PID_STOP 1000
+#define XY_TOLERANCE 300
+#define DIST_BLOQUE 300 //en milimetre
+#define DIST_LIBRE 300 //en milimetre
+#define TEMPS_RECULER 0.5 //en seconde
+
+/*==============================================================================
+                        Déclaration des classes
+==============================================================================*/
+CMPS03 compass(PC_7); //Boussole PWM
+Motor motor_gauche(D10, PB_3, PB_5, 1); //Moteurs de gauche (pwm, fwd, rev)
+Motor motor_droite(PB_4, PB_10, PA_8, 1); //Moteurs de droite (pwm, fwd, rev)
+dwm tag(PA_0, PA_1, 115200); //Balises DWM 1001 UART 
+PID_boussole bouss(&motor_droite, &motor_gauche, &compass); //PID angle
+Serial bt(PC_10, PC_11, 9600); //Bluetooth UART
+InterruptIn button(PC_13); //Interruption sur le bouton
+DigitalOut led(LED1);
+Ticker tick_servo;
+Servo servo_laser(A3);
+//HCSR05 sonar(D10, D11, 0.06);
+/*==============================================================================
+                        Déclaration des variables globales
+==============================================================================*/
+char c;
+int xD_cuisine = 500;
+int yD_cuisine = 2000;
+int xD_salon = 1500;
+int yD_salon = 400;
+int xD;
+int yD;
+int dist_mesure[10];
+int index_avant = 0;
+int tick_servo_en = 1;
+bool tick_sens = true;
+int dest_vue_flag = 1;
+/*==============================================================================
+                        Déclaration des fonctions
+==============================================================================*/
+//La capteur laser fait un balayage de 180 degree
+void mesure_180();
+//Calculer le sens vers la destination par des coordonnees
+double Calcul_Dir(int xR, int yR,  int xD,  int yD);
+/*
+ * INPUT: d: le pointeur vers un tableau de 9 valeurs de distance
+          dirR: la direction du Robot, recuperee par la boussole
+ * OUTPUT: nouvelle_consigne: l'angle absolute pour contourner dse obstacles
+ */
+float contournement(int* d, float dirR, float dirS);
+//Renvoie 1 si la destination est dans notre champ de vision, sinon 0
+int destination_vue(float _xR, float _yR, float _xD, float _yD, int* dist);
+//Renvoie 1 si le robot est arrivé à la destination (avec une certaine tolérance), sinon 0
+int fin_course(int xR, int yR, int xD, int yD);
+//Commander les 2 moteurs pour aller tout droit
+void aller_tout_droit();
+//Initialiser la boussole
+void init_bouss();
+//balayer le servo moteur
+void ftick_servo();
+//la fonction d'interuption sur la reception du module Bluetooth
+void IrqBT();
+//comparer les mesures de distance, renvoyer 1 il y a d'obstacle de tous les sens
+int verifier_bloquer(int* d);
+//comparer les mesures de distance, renvoyer 1 il n'y a PAS d'obstacle de tous les sens
+int verifier_espace(int* d);
+//Commander les 2 moteurs pour reculer
+void robot_reculer();
+
+int main()
+{   
+/*==============================================================================
+                        Déclaration des variables globales
+==============================================================================*/
+    int flag_init = 0;
+    int xR;
+    int yR;
+    int d;
+    int fin = 0;
+    int ret;
+    float angle_direction;
+    float nouvelle_angle;
+    float sensR, sensD;
+    
+    button.rise(&init_bouss); //Interruption (front montant) = init_bouss apelée
+    bt.attach(&IrqBT); //Interruption sur la réception Bluetooth UART = IrqBT appelée
+    
+    while(button != 0){} //Attente d'un front montant sur le bouton de la Nucléo
+
+    motor_gauche.stop(0);
+    motor_droite.stop(0);
 
-CMPS03 compass(PC_7);
-//=== Moteurs (pwm, fwd, rev)
-Motor motor_droite(PA_10, PB_5, PB_3, 1);
-Motor motor_gauche(PB_4, PA_8, PB_10, 1);
-dwm tag(PA_0, PA_1, 115200);
+    bouss.START_PID_boussole(); //Attache le ticker pour calculer l'angle
+    
+    laser_init();
+    servo_laser.set_position(98);
+    wait_ms(300);
+    tick_servo.attach(&ftick_servo, 0.025); //Attache le ticker pour balayer le servo
+    //cf.Algorigramme
+    while(1)
+    {
+        tick_servo_en = fin;
+        ret = tag.dwm_pos_get();
+        if(ret == 0)
+        {
+            xR = tag.node_pos.x;
+            yR = tag.node_pos.y; 
+            if(flag_init == 0)
+            {
+                xD = xR;
+                yD = yR;
+                flag_init = 1;
+            }
+            angle_direction = Calcul_Dir(xR, yR, xD, yD);
+            if(fin == 1) bouss.PID_boussole_consigne(PID_STOP); 
+            else
+            {
+                bouss.PID_boussole_consigne(angle_direction); 
+                d = laser_mesure(2);
+                if(d < safe_distance) 
+                {
+                    motor_gauche.stop(0);
+                    motor_droite.stop(0);
+                    bouss.PID_boussole_consigne(PID_STOP);
+                    dest_vue_flag = 0;
+                }
+                tick_servo_en = 1; //0 __ enable || 1 __disable
+                wait(0.1);
+                while(dest_vue_flag != 1)
+                {
+                    ret = tag.dwm_pos_get();
+                    if(ret == 0)
+                    {
+                        xR = tag.node_pos.x;
+                        yR = tag.node_pos.y; 
+                        motor_gauche.stop(0);
+                        motor_droite.stop(0);
+                        bouss.PID_boussole_consigne(PID_STOP); 
+                        mesure_180();
+                        dest_vue_flag = destination_vue(xR, yR, xD, yD, dist_mesure);
+                        printf("%d  \n\r",dest_vue_flag);
+                        if(dest_vue_flag != 1)
+                        {
+                            sensR = compass.getBearing();
+                            sensD = Calcul_Dir(xR, yR, xD, yD);
+                            if(verifier_bloquer(dist_mesure) == 0)
+                            {
+                                nouvelle_angle = contournement(dist_mesure, sensR, sensD);
+                                robot_reculer();
+                                bouss.PID_boussole_consigne(nouvelle_angle);
+                                printf("CONTOURNER!!!  %.2f\n\r", nouvelle_angle);
+                            }
+                            else
+                            {
+                                robot_reculer();
+                                printf("COINCE!!!\n\r");
+                                if(sensD != PID_STOP)
+                                {
+                                    
+                                    if(sensD - sensR >= 0)
+                                    {
+                                        if(sensR > 330) bouss.PID_boussole_consigne(359);
+                                        else bouss.PID_boussole_consigne(sensR + 30);
+                                    }
+                                    else
+                                    {
+                                        if(sensR < 30) bouss.PID_boussole_consigne(0);
+                                        else bouss.PID_boussole_consigne(sensR - 30);
+                                    }
+                                }
+                            }
+                            wait(1.5);
+                        }
+                    }
+                    else wait(0.1); 
+                }
+                dest_vue_flag = 1;
+                tick_servo_en = 0; //0 __ enable || 1 __disable
+            }
+            fin = fin_course(xR, yR, xD, yD);
+        }
+        else 
+        {
+            bouss.PID_boussole_consigne(PID_STOP);
+            wait(0.11);
+        } 
+    }
+}
 
-PID_boussole bouss(&motor_droite, &motor_gauche, &compass);
+void IrqBT()
+{
+    c = bt.getc(); //Pour lire des données du module bluetooth HC06
+    if(c == '1')//Direction la cuisine
+    {
+        led = 1;
+        xD = xD_cuisine;
+        yD = yD_cuisine;
+    }
+    else
+    {
+        if(c=='0')//Direction le salon
+        {   
+            led = 0;
+            xD = xD_salon;
+            yD = yD_salon;
+        }
+    }
+}
 
-double Calcul_Dir(float xR,float yR,  float xD,  float yD)
+
+double Calcul_Dir(int xR, int yR,  int xD,  int yD)
 {
     double A;
     double Result;
     
-    A =((double)(yD-yR)/(double)(xD-xR));
+    A =((double)(yD-yR)/(double)(xD-xR+0.0001));
     if(xR <= xD)
     {
         if(yR < yD)
@@ -33,70 +239,162 @@
     return Result;
 }
 
-int destination_vue(float _xR,float _yR,  float _xD,  float _yD, int* dist)
+float contournement(int* d, float dirR, float dirD)
+{
+    double k;
+    double angle_tourner;
+    double nouvelle_consigne;
+    double somme_d = 0;
+    double somme_d_k = 0;
+    for(int i=0; i<10; i++)
+    {
+        if(i<5)k = i-5;
+        else k = i-4;
+        somme_d += (float)d[i];
+        somme_d_k += k*(float)d[i];
+    }
+    angle_tourner = (somme_d_k/somme_d)*ALPHA0*GAIN_CONTOURNER;
+    if(angle_tourner>90) angle_tourner=90;
+    if(angle_tourner<-90) angle_tourner=-90;
+    //printf("%.2f\n\r", angle_tourner);
+    //printf("%.2f\n\r", dirR);
+    if(abs(angle_tourner) > 10)
+    {
+        nouvelle_consigne = angle_tourner + dirR;
+        //printf("Bon calcul: %.2f \n\r", nouvelle_consigne);
+    }
+    else 
+    {
+        if(dirD != PID_STOP)
+        {
+            if(dirD - dirR >= 0) nouvelle_consigne = 60 + dirR;
+            else nouvelle_consigne = -60 + dirR;
+            //printf("Mauvais calcul: %.2f \n\r", nouvelle_consigne);
+        } 
+        else nouvelle_consigne = 60 + dirR; //force a tourner a gauche
+    }
+    if(nouvelle_consigne<0) nouvelle_consigne += 360;
+    if(nouvelle_consigne>355) nouvelle_consigne = 355;
+    //printf("%.2f\n\r", nouvelle_consigne);
+    return nouvelle_consigne;
+}
+
+//Renvoie 1 si la destination est dans notre champ de vision, sinon 0
+int destination_vue(float _xR, float _yR, float _xD, float _yD, int* dist)
 {
     double angle_dest;
     double angle_robot;
-    double champ_vision;
-    double dist_robot_dest;
+    int champ_vision;
+    int dist_robot_dest;
+    int d1, d2, d3; //3 distance a comparer autour le sens de la destination
     
     angle_dest = Calcul_Dir(_xR, _yR, _xD, _yD);
     angle_robot = compass.getBearing();
-    champ_vision = 4-((angle_dest - angle_robot)/20.0);
-    dist_robot_dest = sqrt(((_xD-_xR)*(_xD-_xR))+((_yD-_yR)*(_yD-_yR)));//pytaghore
-    
-    if(dist_robot_dest > 120)dist_robot_dest = 120;
-    if(champ_vision > 6.0 && champ_vision < 8.0)
+    champ_vision = (angle_dest - (angle_robot - 90))/20;
+    //si la destination est dans le champ vision 180 degree
+    if((champ_vision > 0) && (champ_vision < 9))
+    {
+        dist_robot_dest = sqrt(((_xD-_xR)*(_xD-_xR))+((_yD-_yR)*(_yD-_yR)));//pythagore
+        if(dist_robot_dest > 1200) dist_robot_dest = 1200;
+        d1 = abs(dist[champ_vision-1] - dist_robot_dest);
+        d2 = abs(dist[champ_vision] - dist_robot_dest);
+        d3 = abs(dist[champ_vision+1] - dist_robot_dest);
+
+        if( ((d1<100) && (d2<100)) || ((d2<100) && (d3<100)))
         {
-            if((dist[7]>(dist_robot_dest-10) && dist[7]<(dist_robot_dest+10)) && (dist[8]>(dist_robot_dest-10) && dist[8]<(dist_robot_dest+10))) return 1;
-        }
-    else if(champ_vision > 5.0 && champ_vision < 7.0)
-         {
-            if((dist[6]>(dist_robot_dest-10) && dist[6]<(dist_robot_dest+10)) && (dist[6]>(dist_robot_dest-10) && dist[6]<(dist_robot_dest+10))) return 1;
-         }
-    else if(champ_vision > 4.0 && champ_vision < 6.0)
-        {
-            if((dist[6]>(dist_robot_dest-10) && dist[6]<(dist_robot_dest+10)) && (dist[5]>(dist_robot_dest-10) && dist[5]<(dist_robot_dest+10))) return 1;
+            return 1;
         }
-    else if(champ_vision > 3.0 && champ_vision < 5.0)
-         {
-            if((dist[5]>(dist_robot_dest-10) && dist[5]<(dist_robot_dest+10)) && (dist[4]>(dist_robot_dest-10) && dist[4]<(dist_robot_dest+10))) return 1;
-         }
-    else if(champ_vision > 2.0 && champ_vision < 4.0)
-         {
-            if((dist[4]>(dist_robot_dest-10) && dist[4]<(dist_robot_dest+10)) && (dist[3]>(dist_robot_dest-10) && dist[3]<(dist_robot_dest+10))) return 1;
-         }
-    else if(champ_vision > 1.0 && champ_vision < 3.0)
-         {
-             if((dist[3]>(dist_robot_dest-10) && dist[3]<(dist_robot_dest+10)) && (dist[2]>(dist_robot_dest-10) && dist[2]<(dist_robot_dest+10))) return 1;
-         } 
-    else {
-            if((dist[1]>(dist_robot_dest-10) && dist[1]<(dist_robot_dest+10)) && (dist[1]>(dist_robot_dest-10) && dist[1]<(dist_robot_dest+10))) return 1;
-         }
+        else return 0;
+    }
+    //verifier si il y a assez d'espace pour mettre dest_vue_flag à 1
+    else 
+    {
+        return verifier_espace(dist);
+    }
+}
+//Renvoie 1 si le robot est arrivé à la destination (avec une certaine tolérance), sinon 0
+int fin_course(int xR, int yR, int xD, int yD)
+{
+    if ((xR >= (xD-XY_TOLERANCE) && xR <= (xD+XY_TOLERANCE)) && (yR >= (yD-XY_TOLERANCE) && yR <= (yD+XY_TOLERANCE)))
+    {
+        bouss.PID_boussole_consigne(PID_STOP); 
+        motor_gauche.stop(0);
+        motor_droite.stop(0);
+        return 1;
+    }
     return 0;
 }
+void aller_tout_droit()
+{
+    motor_gauche.speed(1);
+    motor_droite.speed(0.76);
+}
+//Initialiser la boussole
+void init_bouss(){compass.init = true;}
 
-int main()
-{   
-    int ret; //la valeur de return
-    float angle_direction;
-    bouss.START_PID_boussole();
-    while(1)
+void ftick_servo()
+{
+    if(tick_servo_en == 0) //0 __ enable || 1 __disable
     {
-        ret = tag.dwm_pos_get();
-        if(ret == 0)
-        {
-            angle_direction = Calcul_Dir((float)tag.node_pos.x, (float)tag.node_pos.y, 500.0, 3000.0);
-            bouss.PID_boussole_consigne(angle_direction);
-                //tester Balise)
-            printf("======\n\r");
-            printf("%d\n\r", ret);
-            printf("%d\n\r", tag.node_pos.x);
-            printf("%d\n\r", tag.node_pos.y);
-            printf("%.2f\n\r", angle_direction);
-            printf("%.2f\n\r", compass.getBearing());
-            wait(0.5);
-        }
+        servo_laser.set_position(index_avant*25+40);
+        
+        if(tick_sens) index_avant += 1;
+        else index_avant -= 1;
+        if(index_avant >= 5) tick_sens = false;
+        if(index_avant <= 0) tick_sens = true;
     }
 }
 
+void mesure_180()
+{
+    tick_servo_en = 1; //0 __ enable || 1 __disable
+    wait_ms(10);
+    servo_laser.set_position(0);
+    wait_ms(300);
+    int i;
+    for(i=0; i<10; i++)
+    {
+        servo_laser.set_position(i*ALPHA0);
+        wait(0.06);
+        dist_mesure[i] = laser_mesure(3);
+        if(dist_mesure[i] > 1200) dist_mesure[i] = 1200;
+    }
+    servo_laser.set_position(98);
+    wait_ms(300);
+    
+}
+
+int verifier_bloquer(int* d)
+{
+    int i;
+    int somme = 0;
+    for(i=0; i<10; i++)
+    {
+        if(d[i] < DIST_BLOQUE) somme += 1;
+    }
+    if(somme < 10) return 0;
+    return 1;
+}
+
+int verifier_espace(int* d)
+{
+    int i;
+    int somme = 0;
+    for(i=0; i<10; i++)
+    {
+        if(d[i] >= DIST_LIBRE) somme += 1;
+        //printf("%d \n\r",d[i]);
+    }
+    if(somme < 10) return 0;
+    return 1;
+}
+
+void robot_reculer()
+{
+    bouss.PID_boussole_consigne(PID_STOP); 
+    motor_gauche.speed(-1);
+    motor_droite.speed(-0.76);
+    wait(TEMPS_RECULER);
+    motor_gauche.stop(0);
+    motor_droite.stop(0);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/vl53l0x_api.lib	Wed Apr 03 22:49:33 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/nguyentony/code/vl53l0x_api/#74763ee81c17