Revision 20:7d206773f39e, committed 2020-10-20

Comitter:

Nanaud

Date:

Tue Oct 20 17:53:32 2020 +0000

Parent:

19:c419033c0967

Child:

21:e5f0f5abb5ae

Commit message:

Fonctionnel, detection a ameliorer

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Flag.cpp	Tue Oct 20 17:53:32 2020 +0000
@@ -0,0 +1,52 @@
+#include "pins.h"
+
+PwmOut servo(FLAGPIN);
+Timeout response;
+
+
+void FlagGOTO(int pos)
+{
+    servo = (pos*(0.05f)/90)+0.075f;
+}
+
+void FlagFROMGOTO(int posinit, int posfin, int pas)
+{
+    int dir = ((posfin - posinit)/abs(posfin - posinit));
+    for(int pos = posinit; pos != posfin; pos = pos + (pas*dir)) {
+        FlagGOTO(pos);
+        wait_ms(FLAGDELAY2POS);
+    }
+}
+
+void FlagCCW(void)
+{
+    FlagGOTO(90);
+    //servo = 0.125f;
+}
+
+void FlagMid(void)
+{
+    FlagGOTO(0);
+    //servo = 0.075f;
+}
+
+void FlagCW(void)
+{
+    FlagGOTO(-90);
+    //servo = 0.024f;
+}
+
+void FlagUpToInit()
+{
+    FlagFROMGOTO(0, 90, 1);
+}
+
+void FlagInitToUp()
+{
+    FlagFROMGOTO(90, 0, 1);
+}
+
+void Detonateur(float time)
+{
+    response.attach(&FlagInitToUp, time);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Flag.h	Tue Oct 20 17:53:32 2020 +0000
@@ -0,0 +1,16 @@
+//#include "mbed.h"
+#include <stdlib.h>
+ 
+#define FLAGDELAY 97
+#define FLAGDELAY2POS 10
+#define FLAGPIN PA_10
+ 
+void FlagCCW(void);
+void FlagMid(void);
+void FlagCW(void);
+void FlagFROMGOTO(int, int, int);
+void FlagGOTO(int);
+ 
+void Detonateur(float);
+void FlagUpToInit(void);
+void FlagInitToUp(void);
\ No newline at end of file

--- 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;
+}

--- a/captUS.h	Mon Oct 12 19:17:40 2020 +0000
+++ b/captUS.h	Tue Oct 20 17:53:32 2020 +0000
@@ -1,9 +1,23 @@
 // Nom du fichier : captUS.h
 
+#ifndef CAPTUS_H
+#define CAPTUS_H
+//#include "mbed.h"
+#include "math.h"
+
+#define LONGUEUR_TAB 2000
+#define LARGEUR_TAB 3000
+#define _PI_ 3.14159265359
+#define THETA 60
+
 // extern
 extern float us_out[6];
 extern Timer tps;
 extern Ticker ticker_US;
+extern float* distance;
+
+extern bool rebooted;
+extern bool wtt;
 
 // Prototypes
 void captUS_trig();
@@ -20,4 +34,29 @@
 void echoRise5();
 void echoFall5();
 void echoRise6();
-void echoFall6();
\ No newline at end of file
+void echoFall6();
+
+/**********************************
+ * Création d'une fonction qui    *
+ * convertis le temps en distance *
+ **********************************/
+float* convertToDistance();
+ 
+ 
+/********************************************
+ * nous permet de placer l'origine au coins *
+ * de la table                              *
+ ********************************************/
+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);
+ 
+ 
+ 
+ 
+#endif // CAPTUS_H
\ No newline at end of file

--- a/debugPC.cpp	Mon Oct 12 19:17:40 2020 +0000
+++ b/debugPC.cpp	Tue Oct 20 17:53:32 2020 +0000
@@ -189,12 +189,14 @@
             //xC = (double)0;
             //yC = (double)100;
             //fnc=1;
-            
+
+            /*
             indice++;
             fnc = objEtape[indice];
             xC = objX[indice];
             yC = objY[indice];
             mot_en();
+            */
             break;
         case 9:     //cdon
             pc.printf("Results ALL Encoders ON/OFF\n\r");
@@ -303,6 +305,10 @@
             //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("fnc = %d\n\r", fnc);
             break;
         default:
             pc.printf("Commande invalide\n\r");
@@ -378,6 +384,9 @@
 
 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]);
@@ -395,6 +404,28 @@
     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]||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("\n\r");
+
+    // bt
+    /*
+    bt.printf("distance[0] = %f\n\r", ::distance[0]);
+    bt.printf("distance[1] = %f\n\r", ::distance[1]);
+    bt.printf("distance[2] = %f\n\r", ::distance[2]);
+    bt.printf("distance[3] = %f\n\r", ::distance[3]);
+    bt.printf("distance[4] = %f\n\r", ::distance[4]);
+    bt.printf("distance[5] = %f\n\r", ::distance[5]);
+    */
 }
 
 void affCodeurs()

--- a/main.cpp	Mon Oct 12 19:17:40 2020 +0000
+++ b/main.cpp	Tue Oct 20 17:53:32 2020 +0000
@@ -1,6 +1,16 @@
 // Nom du fichier : main.cpp
 #include "pins.h"
 
+//#define TPS_DRAPEAU 20 // 95
+//#define TPS_FIN 100
+
+Ticker TimerGlobal;
+int cptGlobal = 0;
+
+bool Match = 0;
+DigitalOut led(LED1);
+DigitalIn tirette(PC_8);
+
 void btnFct()
 {
     mot_dis();
@@ -13,6 +23,19 @@
     bt.printf("comptD = %d\r\n",comptD);
 }
 
+void fctCptGlobal()
+{
+    cptGlobal++;
+    
+    if(cptGlobal==95) {
+        FlagGOTO(0);
+    }
+    
+    if(cptGlobal>=100) {
+        fnc=0;
+    }
+}
+
 /*
 void cordonDem()
 {
@@ -21,16 +44,24 @@
     xC = objX[indice];
     yC = objY[indice];
     mot_en();
+    myled = 0;
 }
 */
 
+
 int main()
 {
+    captUS_init();
+    FlagGOTO(90);
+    TimerGlobal.attach(&fctCptGlobal, 1.0);
+    
+    tirette.mode(PullDown);
+    //myled = 1;
+
     pc.printf("\r\nAresCDFMainCode\r\n");
     bt.printf("\r\nAresCDFMainCode\r\n");
 
     btn.rise(&btnFct);
-    //cordon.fall(&cordonDem);
 
     // debug
     pc.attach(&serialIT); // Interruption liaison série
@@ -40,7 +71,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.2); // On apelle cette fonction toutes 0.2 secondes
     //ticker_affUS.attach(&affUltrasons,1.0);
     //ticker_affcd.attach(&affCodeurs,1.0);
     //ticker_odo.attach(&odo2,0.02);
@@ -72,9 +103,26 @@
     cddA.mode(PullUp);
 
     while(1) {
-        if (indice>=4) {
+        if (tirette == 1 && Match == 0) {
+            Match = 0;
+            led = 1;
+        } else if (tirette == 0 && Match == 0){
+            Match = 1;
+            
+            led = 0;
+
+            //indice++;
+            indice=1;
+            fnc = objEtape[indice];
+            xC = objX[indice];
+            yC = objY[indice];
+            cptGlobal = 0;
+            //mot_en();
+        }
+
+        if (indice>=NbObj) {
             fnc = 0;
-            mot_dis();
+            //mot_dis();
         }
 
     }

--- a/odo_asserv.cpp	Mon Oct 12 19:17:40 2020 +0000
+++ b/odo_asserv.cpp	Tue Oct 20 17:53:32 2020 +0000
@@ -1,15 +1,20 @@
 //Nom du fichier : odo_asserv.cpp
 #include "pins.h"
 
-#define VMAXROT 0.060 // 0.030
-#define VMAXLIN 0.100 // 0.050
+#define VMAXROT 0.050 // 0.030
+#define VMAXLIN 0.130 // 0.050
+
+//const float DISTLIM = 150.0;
+
+int cptErreur = 0;
 
 // Objectifs
-#define NbObj 4
+//#define NbObj 5
 int indice = 0;
-int objEtape [4] = {0,1,1,1};
-double objX[4] = {0,660, 660, 210};
-double objY[4] = {0,1070,1650,1300};
+int objEtape[NbObj] = {0,1,1,1,1,1,1,1};
+double objX[NbObj] = {0,300,660,660,210,660,400,210};
+double objY[NbObj] = {0,1070,1270,1650,1300,1650,1800,1300};
+int objRecule[NbObj] = {0,0,0,0,0,1,0,0};
 
 ///// VARIABLES
 Ticker ticker_odo;
@@ -17,10 +22,11 @@
 
 // Coeff à définir empiriquement
 const double coeffGLong = 5.956, coeffDLong = 5.956; // constantes permettant la transformation tic/millimètre
-const double coeffGAngl = 737.447, coeffDAngl = 748.057; // constantes permettant la transformation tic/radian
+//const double coeffGAngl = 737.447, coeffDAngl = 748.057; // constantes permettant la transformation tic/radian
 
 long comptG = 0, comptD = 0; // nb de tics comptés pour chaque codeur
 
+//bool objOnce = 0;
 
 ///// INTERRUPTIONS CODEURS
 
@@ -36,8 +42,6 @@
     else comptD++;
 }
 
-
-
 ///*
 // odo2()
 //#define diametre 51.45 // 51.45 théorique
@@ -59,11 +63,9 @@
     y += (double) dDist * sin(O);
     O += (double) dAngl;
 
-
     if (O > 3.1415) O = O - (2.0f * 3.1415f);
     if (O < -3.1415) O = O + (2.0f * 3.1415f);
 
-
     comptG = 0;
     comptD = 0;
 }
@@ -79,21 +81,24 @@
 double erreurAngle = 0;
 double erreurPre = 0;
 double deltaErreur = 0;
-const double coeffPro = 0.08; // 0.023 de base
-const double coeffDer = 0.06; // 0.023 de base
+const double coeffPro = 0.075; // 0.023 de base
+const double coeffDer = 0.060; // 0.023 de base
 
 // Ligne droite
 //double erreurDist = 0;
 double erreurPreDist = 0;
 double deltaErreurDist = 0;
-const double coeffProDist = 0.10; // 0.023 de base
-const double coeffDerDist = 0.10; // 0.023 de base
+const double coeffProDist = 0.0005; // 0.010 de base
+const double coeffDerDist = 0.0005; // 0.010 de base
 
 // NEW NEW NEW NEW
 int fnc = 0;
 bool acc = 1;
 double distancePrecedente = 0;
-bool stt = 0;
+bool stt = 0; // Dépassement du point
+
+// Reculer
+double distanceFaite = 0;
 
 void asserv()
 {
@@ -126,19 +131,83 @@
         case 1: // Rotation
             mot_en();
 
+            // Si on doit reculer
+            if (objRecule[indice]==1) {
+                consigneOrientation += 3.1415;
+
+                if(consigneOrientation>3.1415) consigneOrientation-=2*3.1415;
+                if(consigneOrientation<-3.1415) consigneOrientation+=2*3.1415;
+            }
+
             // Choix du sens de rotation
-            double choixSens = consigneOrientation - O;
 
-            if (choixSens > 3.1415) choixSens -= (2.0f * 3.1415f);
-            else if (choixSens < 3.1415) choixSens += (2.0f * 3.1415f);
+            double Osens = 0;
+            if (O<0) Osens = O + (2.0f*3.1415f);
+            else Osens = O;
+
+            double consigneSens = 0;
+            if (consigneOrientation<0) consigneSens = consigneOrientation + (2.0f*3.1415f);
+            else consigneSens = consigneOrientation;
 
-            if (choixSens > 0) {
+            double choixSens = consigneSens - Osens;
+
+            //if(objRecule[indice] == 0) {
+            //if (choixSens > 3.1415) choixSens -= (2.0f * 3.1415f);
+            //else if (choixSens < 3.1415) choixSens += (2.0f * 3.1415f);
+
+            if ((choixSens > 0) && (choixSens <= 3.1415) || (choixSens<(-3.1415))) {
                 motGauche_bck();
                 motDroite_fwd();
             } else {
                 motGauche_fwd();
                 motDroite_bck();
             }
+            //}
+
+            /*
+            if(objRecule[indice] == 1) {
+                //if (choixSens > 3.1415) choixSens -= (2.0f * 3.1415f);
+                //else if (choixSens < 3.1415) choixSens += (2.0f * 3.1415f);
+
+                if (choixSens > 0) {
+                    motGauche_fwd();
+                    motDroite_bck();
+                } else {
+                    motGauche_bck();
+                    motDroite_fwd();
+                }
+            }
+            */
+
+            /*
+            double choixSens = consigneOrientation - O;
+
+            if(objRecule[indice] == 0) {
+                if (choixSens > 3.1415) choixSens -= (2.0f * 3.1415f);
+                else if (choixSens < 3.1415) choixSens += (2.0f * 3.1415f);
+
+                if (choixSens > 0) {
+                    motGauche_bck();
+                    motDroite_fwd();
+                } else {
+                    motGauche_fwd();
+                    motDroite_bck();
+                }
+            }
+
+            if(objRecule[indice] == 1) {
+                if (choixSens > 3.1415) choixSens -= (2.0f * 3.1415f);
+                else if (choixSens < 3.1415) choixSens += (2.0f * 3.1415f);
+
+                if (choixSens > 0) {
+                    motGauche_fwd();
+                    motDroite_bck();
+                } else {
+                    motGauche_bck();
+                    motDroite_fwd();
+                }
+            }
+            */
 
             // Asservissement en position angulaire
             erreurAngle =  consigneOrientation - O;
@@ -150,7 +219,7 @@
             double deltaCommande = (abs(coeffPro * erreurAngle) + abs(coeffDer * deltaErreur));
 
             if(acc) {
-                cmdG = cmdG + 0.0005;
+                cmdG = cmdG + 0.0007; // +0.0008
                 cmdD = cmdG;
 
                 if (cmdG >= VMAXROT) acc = 0;
@@ -172,12 +241,59 @@
             if (O > (consigneOrientation - (1*0.0174533)) && O < (consigneOrientation + (1*0.0174533))) {
                 //mot_dis();
                 fnc++;
+                rebooted = 1;
                 acc = 1;
+                //objOnce = 0;
+                break;
             }
+
             break;
 
         case 2: // Avancer
-            mot_en();
+        
+            if (rebooted == 1 && wtt==1) {
+                cmdG = 0;
+                cmdD = 0;
+                rebooted = 0;
+                acc=1;
+                mot_en();
+            }
+
+            /*
+            if(objRecule[indice]==0) {
+                if ((::distance[0] >= 120) && (::distance[1] >= 140) && (::distance[5] >= 140)) {
+                    mot_en();
+                    cptErreur=0;
+
+                } else {
+                    cptErreur++;
+
+                    if(cptErreur>=5) {
+                        mot_dis();
+                        acc = 1;
+                    }
+
+                    break;
+                }
+            }
+
+            else if(objRecule[indice]==1) {
+                if ((::distance[2] >= 140) && (::distance[3] >= 120) && (::distance[4] >= 140)) {
+                    mot_en();
+                    cptErreur=0;
+
+                } else {
+                    cptErreur++;
+
+                    if(cptErreur>=5) {
+                        mot_dis();
+                        acc = 1;
+                    }
+
+                    break;
+                }
+            }
+            */
 
             /*
             cmdD = abs((int)distanceCible)*0.0001; // *0.005
@@ -197,12 +313,12 @@
             double deltaCommande2 = (abs(coeffProDist * distanceCible) + abs(coeffDerDist * deltaErreurDist));
 
             if(acc) {
-                cmdG = cmdG + 0.0005;
+                cmdG = cmdG + 0.0012; // +0.0008
                 cmdD = cmdG;
 
                 if (cmdG >= VMAXLIN) {
                     acc = 0;
-                    stt = 1;
+                    //stt = 1;
                 }
             } else {
                 //acc = 0;
@@ -216,33 +332,83 @@
                 }
             }
 
-            vitesseMotG(abs(cmdG));
-            vitesseMotD(abs(cmdD));
+            //vitesseMotG(abs(cmdG));
+            //vitesseMotD(abs(cmdD));
 
             //if (distanceCible < 15)[
             //if ((x < xC+10) && (x > xC-10) && (y < yC+10) && (y > yC-10)) {
-            if ((distanceCible < 10) || (stt==1 && (distancePrecedente < distanceCible))) {
+            if ((distanceCible < 10) || (acc==0 && wtt==1 && rebooted==0 && (distancePrecedente < distanceCible))) {
                 //mot_dis();
-                //fnc=0;
+                //fnc++;
                 acc = 1;
-                stt = 0;
+                //stt = 0;
+
+                //if (objOnce == 0) {
                 indice++;
-                fnc = objEtape[indice];
-                xC = objX[indice];
-                yC = objY[indice];
+                //objOnce=1;
+
+
+
+                //}
+
+                if (indice>=(int)NbObj) {
+                    fnc = 0;
+                } else {
+                    fnc = objEtape[indice];
+                    xC = objX[indice];
+                    yC = objY[indice];
+                }
+
+                distancePrecedente = 0;
+                break;
+
+                /*
+                if (indice>=NbObj) {
+                    fnc = 0;
+                } else {
+                    fnc = objEtape[indice];
+                    xC = objX[indice];
+                    yC = objY[indice];
+                }
+                */
             }
 
-            motGauche_fwd();
-            motDroite_fwd();
+            if (objRecule[indice] == 0) {
+                motGauche_fwd();
+                motDroite_fwd();
+            } else {
+                motGauche_bck();
+                motDroite_bck();
+            }
+
             vitesseMotG(cmdG);
             vitesseMotD(cmdD);
 
             distancePrecedente = distanceCible;
-
             break;
 
+        /*
         case 3: // Reculer
+
+                if (objRecule[indice] == 1) {
+                    distanceFaite += abs(dDist);
+
+                    motGauche_bck();
+                    motDroite_bck();
+                    vitesseMotG(0.01);
+                    vitesseMotD(0.01);
+                }
+
+                if ((distanceFaite >= 100) || (objRecule[indice] == 0)) {
+                    indice++;
+                    fnc = objEtape[indice];
+                    xC = objX[indice];
+                    yC = objY[indice];
+                    distanceFaite = 0;
+                }
+
             break;
+            */
 
         default:
             mot_dis();

--- a/odo_asserv.h	Mon Oct 12 19:17:40 2020 +0000
+++ b/odo_asserv.h	Tue Oct 20 17:53:32 2020 +0000
@@ -1,9 +1,15 @@
 // Nom du fichier : odo_asserv.h
 
+#define NbObj 8
+
 extern int indice;
-extern int objEtape[4];
-extern double objX[4];
-extern double objY[4];
+extern int objEtape[NbObj];
+extern double objX[NbObj];
+extern double objY[NbObj];
+extern int objRecule[NbObj];
+
+extern bool acc;
+extern bool stt;
 
 // extern
 // CODEURS

--- a/pins.cpp	Mon Oct 12 19:17:40 2020 +0000
+++ b/pins.cpp	Tue Oct 20 17:53:32 2020 +0000
@@ -2,7 +2,8 @@
 #include "pins.h"
 
 InterruptIn btn(PC_13);
-//InterruptIn cordon(PC_8);
+//DigitalOut myled(LED2);
+//DigitalIn cordon(PC_8);
 
 // Capteurs à ultrasons
 DigitalOut trigger(PB_9);

--- a/pins.h	Mon Oct 12 19:17:40 2020 +0000
+++ b/pins.h	Tue Oct 20 17:53:32 2020 +0000
@@ -8,6 +8,7 @@
 #include "captUS.h"
 #include "motors.h"
 #include "odo_asserv.h"
+#include "Flag.h"
 
 // #define
 //#define Pi 3.14159265359
@@ -20,7 +21,8 @@
 #define perimetreE Pi*ecartCodeuse
 
 extern InterruptIn btn;
-//extern InterruptIn cordon;
+//extern DigitalOut myled;
+//extern DigitalIn cordon;
 
 //Capteurs à ultrasons
 extern DigitalOut trigger;