Revision 22:f891c2bce091, committed 2020-10-27

Comitter:

Nanaud

Date:

Tue Oct 27 17:27:33 2020 +0000

Parent:

21:e5f0f5abb5ae

Child:

23:a74135a0271d

Commit message:

Homologation J-2

Changed in this revision

--- a/captUS.cpp	Sun Oct 25 22:36:51 2020 +0000
+++ b/captUS.cpp	Tue Oct 27 17:27:33 2020 +0000
@@ -1,16 +1,18 @@
-//Nom du fichier : captUS.cpp
+/* #include */
 #include "pins.h"
-const float DISTLIM = 450; //160
+
+/* Variables globales */
+Timer TimUS;
+Ticker TickUS_actu;
 
-// Variables globales & timers
-double us_high[6]={0};
-double us_low[6]={0};
-double us_diff[6]={0};
-bool us_verif[6]={0};
+const float DISTLIM = 450;
+
+double us_high[6]= {0};
+double us_low[6]= {0};
+double us_diff[6]= {0};
+bool us_verif[6]= {0};
 
 double distt[6];
-Timer tps;
-Ticker ticker_US;
 bool rebooted = 0;
 bool wtt = 0;
 
@@ -30,7 +32,8 @@
     convertToDistance();
 
     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)) {
+        if (((distt[5] >= DISTLIM) && (distt[0] >= DISTLIM)  && (distt[1] >= DISTLIM)) || ((distt[5] < 0) && (distt[0] < 0)  && (distt[1] < 0))) {
             wtt = 1;
         }
 
@@ -42,7 +45,8 @@
     }
 
     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)) {
+            if (((distt[2] >= DISTLIM) && (distt[3] >= DISTLIM)  && (distt[4] >= DISTLIM)) || ((distt[2] < 0) && (distt[3] < 0)  && (distt[4] < 0))) {
             wtt=1;
         }
 
@@ -73,7 +77,7 @@
     pc.printf("\n\r");
     */
 
-    tps.reset();
+    TimUS.reset();
     trigger=1;
     wait(0.00002);
     trigger=0;
@@ -82,7 +86,7 @@
 void echoRise1()
 {
     if(us_verif[0] == 0) {
-        us_high[0]=tps.read_us();
+        us_high[0]=TimUS.read_us();
         us_verif[0] = 1;
     }
 }
@@ -90,7 +94,7 @@
 void echoFall1()
 {
     if(us_verif[0] == 1) {
-        us_low[0]=tps.read_us();
+        us_low[0]=TimUS.read_us();
         us_diff[0]=us_low[0]-us_high[0];
         us_verif[0] = 0;
     }
@@ -99,7 +103,7 @@
 void echoRise2()
 {
     if(us_verif[1] == 0) {
-        us_high[1]=tps.read_us();
+        us_high[1]=TimUS.read_us();
         us_verif[1] = 1;
     }
 }
@@ -107,7 +111,7 @@
 void echoFall2()
 {
     if(us_verif[1] == 1) {
-        us_low[1]=tps.read_us();
+        us_low[1]=TimUS.read_us();
         us_diff[1]=us_low[1]-us_high[1];
         us_verif[1] = 0;
     }
@@ -116,7 +120,7 @@
 void echoRise3()
 {
     if(us_verif[2] == 0) {
-        us_high[2]=tps.read_us();
+        us_high[2]=TimUS.read_us();
         us_verif[2] = 1;
     }
 }
@@ -124,7 +128,7 @@
 void echoFall3()
 {
     if(us_verif[2] == 1) {
-        us_low[2]=tps.read_us();
+        us_low[2]=TimUS.read_us();
         us_diff[2]=us_low[2]-us_high[2];
         us_verif[2] = 0;
     }
@@ -133,7 +137,7 @@
 void echoRise4()
 {
     if(us_verif[3] == 0) {
-        us_high[3]=tps.read_us();
+        us_high[3]=TimUS.read_us();
         us_verif[3] = 1;
     }
 }
@@ -141,7 +145,7 @@
 void echoFall4()
 {
     if(us_verif[3] == 1) {
-        us_low[3]=tps.read_us();
+        us_low[3]=TimUS.read_us();
         us_diff[3]=us_low[3]-us_high[3];
         us_verif[3] = 0;
     }
@@ -150,7 +154,7 @@
 void echoRise5()
 {
     if(us_verif[4] == 0) {
-        us_high[4]=tps.read_us();
+        us_high[4]=TimUS.read_us();
         us_verif[4] = 1;
     }
 }
@@ -158,7 +162,7 @@
 void echoFall5()
 {
     if(us_verif[4] == 1) {
-        us_low[4]=tps.read_us();
+        us_low[4]=TimUS.read_us();
         us_diff[4]=us_low[4]-us_high[4];
         us_verif[4] = 0;
     }
@@ -167,7 +171,7 @@
 void echoRise6()
 {
     if(us_verif[5] == 0) {
-        us_high[5]=tps.read_us();
+        us_high[5]=TimUS.read_us();
         us_verif[5] = 1;
     }
 }
@@ -175,7 +179,7 @@
 void echoFall6()
 {
     if(us_verif[5] == 1) {
-        us_low[5]=tps.read_us();
+        us_low[5]=TimUS.read_us();
         us_diff[5]=us_low[5]-us_high[5];
         us_verif[5] = 0;
     }

--- a/captUS.h	Sun Oct 25 22:36:51 2020 +0000
+++ b/captUS.h	Tue Oct 27 17:27:33 2020 +0000
@@ -18,8 +18,8 @@
 extern double us_diff[6];
 extern bool us_verif[6];
 
-extern Timer tps;
-extern Ticker ticker_US;
+extern Timer TimUS;
+extern Ticker TickUS_actu;
 extern double distt[6];
 
 extern bool rebooted;

--- a/debug.h	Sun Oct 25 22:36:51 2020 +0000
+++ b/debug.h	Tue Oct 27 17:27:33 2020 +0000
@@ -6,9 +6,7 @@
 // extern
 extern Serial pc;
 extern Serial bt;
-extern Ticker ticker_affUS;
-extern Ticker ticker_affcd;
-extern Ticker ticker_affodo;
+extern Ticker Ticker_affUS;
 
 extern bool aff_US[6];
 extern bool aff_cd[4];

--- a/debugPC.cpp	Sun Oct 25 22:36:51 2020 +0000
+++ b/debugPC.cpp	Tue Oct 27 17:27:33 2020 +0000
@@ -1,11 +1,9 @@
 // Nom du fichier : debugPC.cpp
 #include "pins.h"
 
-// Variables globales & timers
+// Variables globales
 Serial pc(USBTX, USBRX);
-Ticker ticker_affUS;
-Ticker ticker_affcd;
-Ticker ticker_affodo;
+Ticker Ticker_affUS;
 
 bool aff_US[6];
 bool aff_cd[4];
@@ -291,9 +289,16 @@
             pc.printf("x = %lf\n\r", x);
             pc.printf("y = %lf\n\r", y);
             pc.printf("O = %lf\n\r", O);
-            //pc.printf("O = %lf\n\r", (O*180)/_PI_);
-            //pc.printf("vitG = %lf\n\r", vitG);
-            //pc.printf("vitD = %lf\n\r", vitD);
+            pc.printf("consigneAngl = %lf\n\r", consigneOrientation);
+            pc.printf("consigneDist = %lf\n\r", distanceCible);
+            pc.printf("indice = %d\n\r", indice);
+            pc.printf("dist 1 = %lf\n\r", distt[0]);
+            pc.printf("dist 2 = %lf\n\r", distt[1]);
+            pc.printf("dist 6 = %lf\n\r", distt[5]);
+            pc.printf("---\n\r");
+            pc.printf("dist 3 = %lf\n\r", distt[2]);
+            pc.printf("dist 4 = %lf\n\r", distt[3]);
+            pc.printf("dist 5 = %lf\n\r", distt[4]);
 
             bt.printf("Odometrie :\n\r");
             bt.printf("x = %lf\n\r", x);
@@ -302,13 +307,15 @@
             bt.printf("consigneAngl = %lf\n\r", consigneOrientation);
             bt.printf("consigneDist = %lf\n\r", distanceCible);
             bt.printf("indice = %d\n\r", indice);
-            //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);
+            bt.printf("dist 1 = %lf\n\r", distt[0]);
+            bt.printf("dist 2 = %lf\n\r", distt[1]);
+            bt.printf("dist 6 = %lf\n\r", distt[5]);
+            bt.printf("---\n\r");
+            bt.printf("dist 3 = %lf\n\r", distt[2]);
+            bt.printf("dist 4 = %lf\n\r", distt[3]);
+            bt.printf("dist 5 = %lf\n\r", distt[4]);
+            bt.printf("distanceMem = %lf\n\r", distanceMem);
+            bt.printf("distancePlus = %lf\n\r", distancePlus);
             break;
         default:
             pc.printf("Commande invalide\n\r");
@@ -410,16 +417,27 @@
     // Distances
 
     // pc
-    pc.printf("distance[0] = %lf\n\r", distt[0]);
-    pc.printf("distance[1] = %lf\n\r", distt[1]);
-    pc.printf("distance[2] = %lf\n\r", distt[2]);
-    pc.printf("distance[3] = %lf\n\r", distt[3]);
-    pc.printf("distance[4] = %lf\n\r", distt[4]);
-    pc.printf("distance[5] = %lf\n\r", distt[5]);
+    /*
+    pc.printf("US[1] = %lf\n\r", us_diff[0]);
+    pc.printf("US[2] = %lf\n\r", us_diff[1]);
+    pc.printf("US[3] = %lf\n\r", us_diff[2]);
+    pc.printf("US[4] = %lf\n\r", us_diff[3]);
+    pc.printf("US[5] = %lf\n\r", us_diff[4]);
+    pc.printf("US[6] = %lf\n\r", us_diff[5]);
     pc.printf("\n\r");
+    */
+    
+    ///*
+    pc.printf("distance[1] = %lf\n\r", distt[0]);
+    pc.printf("distance[2] = %lf\n\r", distt[1]);
+    pc.printf("distance[3] = %lf\n\r", distt[2]);
+    pc.printf("distance[4] = %lf\n\r", distt[3]);
+    pc.printf("distance[5] = %lf\n\r", distt[4]);
+    pc.printf("distance[6] = %lf\n\r", distt[5]);
+    pc.printf("\n\r");
+    //*/
 
     // bt
-
     /*
     bt.printf("distance[0] = %f\n\r", ::distance[0]);
     bt.printf("distance[1] = %f\n\r", ::distance[1]);

--- a/main.cpp	Sun Oct 25 22:36:51 2020 +0000
+++ b/main.cpp	Tue Oct 27 17:27:33 2020 +0000
@@ -1,87 +1,53 @@
-// Nom du fichier : main.cpp
+/* Équipe : Ares ENSEA
+ *  Coupe de France de robotique 2020
+ */
+
+
+/* #include */
 #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();
-    aff_cd[0] = 0;
-    aff_cd[1] = 0;
-
-    pc.printf("comptG = %d\r\n",comptG);
-    pc.printf("comptD = %d\r\n",comptD);
-    bt.printf("comptG = %d\r\n",comptG);
-    bt.printf("comptD = %d\r\n",comptD);
-}
 
-void fctCptGlobal()
-{
-    cptGlobal++;
-    
-    if(cptGlobal==5) { //95
-        FlagGOTO(0);
-    }
-    
-    if(cptGlobal>=100) {
-        fnc=0;
-    }
-}
+/* Variables globales */
+Ticker TickerGlobal; // Comptage du temps écoulé
+int cptGlobal = 0;
+bool match = 0; // Match == 1 => Le match est en cours
 
-/*
-void cordonDem()
-{
-    indice++;
-    fnc = objEtape[indice];
-    xC = objX[indice];
-    yC = objY[indice];
-    mot_en();
-    myled = 0;
-}
-*/
+
+/* Prototypes */
+void btnBlueIT();
+void globalCounterIT();
 
 
 int main()
 {
-    //captUS_init();
-    tps.start();
-    FlagGOTO(90);
-    TimerGlobal.attach(&fctCptGlobal, 1.0);
-    
-    tirette.mode(PullDown);
-    //myled = 1;
+    pc.printf("\r\nAresCDF2020\r\n");
+    bt.printf("\r\nAresCDF2020\r\n");
+
+    /* Initialisation des drivers */
+    drv_init();
 
-    pc.printf("\r\nAresCDFMainCode\r\n");
-    bt.printf("\r\nAresCDFMainCode\r\n");
-
-    btn.rise(&btnFct);
+    /* Initialisation des codeurs */
+    cdgA.rise(&cdgaRise);
+    cddA.rise(&cddaRise);
+    cdgA.mode(PullUp);
+    cddA.mode(PullUp);
 
-    // debug
-    pc.attach(&serialIT); // Interruption liaison série
-    bt.attach(&bluetoothIT); // Interruption  bluetooth
-    pc.baud(9600);
-    pc.format(8,SerialBase::None,1);
-    bt.baud(9600);
-    bt.format(8,SerialBase::None,1);
+    /* Initialisation des broches */
+    Tirette.mode(PullDown);
+
+    /* Initialisation des pavillons */
+    FlagGOTO(90);
 
-    ticker_US.attach(&captUS_trig,0.1); // 0.2
-    //ticker_affUS.attach(&affUltrasons,1.0);
-    //ticker_affcd.attach(&affCodeurs,1.0);
-    //ticker_odo.attach(&odo2,0.02);
-    ticker_asserv.attach(&asserv,0.015);
-    //ticker_affodo.attach(&affOdo,1.0);
+    /* Timers & tickers */
+    TickerGlobal.attach(&globalCounterIT, 1.0);
+    TimUS.start();
 
-    // Init capteurs à ultrasons
-    //captUS_init();
-    echo1.rise(&echoRise1);
+    TickUS_actu.attach(&captUS_trig,0.2);
+    Ticker_asserv.attach(&asserv,0.015);
+    //Ticker_affUS.attach(&affUltrasons,1.0);
+
+    /* Initialisation des capteurs à ultrasons */
+    echo1.rise(&echoRise1); // Interruptions pour les capteurs à ultrasons
     echo1.fall(&echoFall1);
     echo2.rise(&echoRise2);
     echo2.fall(&echoFall2);
@@ -94,37 +60,61 @@
     echo6.rise(&echoRise6);
     echo6.fall(&echoFall6);
 
-    // Init DRV8825
-    drv_init();
+    /* debug */
+    pc.attach(&serialIT); // Interruption liaison série
+    bt.attach(&bluetoothIT); // Interruption bluetooth
+    pc.baud(9600);
+    pc.format(8,SerialBase::None,1);
+    bt.baud(9600);
+    bt.format(8,SerialBase::None,1);
 
-    // Init codeurs
-    cdgA.rise(&cdgaRise);
-    cddA.rise(&cddaRise);
-    cdgA.mode(PullUp);
-    cddA.mode(PullUp);
+    /* Autres */
+    //btnBlue.rise(&boutonBleu);
 
+    /* Boucle infinie */
     while(1) {
-        if (tirette == 1 && Match == 0) {
-            Match = 0;
-            led = 1;
-        } else if (tirette == 0 && Match == 0){
-            Match = 1;
+        if (Tirette == 1 && match == 0) { // Stand-by
+            // Le match n'a pas encore commencé
+            // La LED est allumée
+            // Le compteur du match reste à 0
+            Led = 1;
+            match = 0;
+            cptGlobal=0;
+        }
+
+        else if (Tirette == 0 && match == 0) { // Début du match
+            // Le match débute
+            // La LED s'éteind
+            Led = 0;
+            match = 1;
+            cptGlobal = 0;
             
-            led = 0;
-
-            //indice++;
-            indice=1;
+            indice= 1;
             fnc = objEtape[indice];
             xC = objX[indice];
             yC = objY[indice];
-            cptGlobal = 0;
-            //mot_en();
         }
 
         if (indice>=NbObj) {
             fnc = 0;
-            //mot_dis();
         }
-
     }
 }
+
+void btnBlueIT()
+{
+    /* Rien */
+}
+
+void globalCounterIT()
+{
+    cptGlobal++;
+
+    if(cptGlobal==95) { // Le drapeau doit être hissé à 95sec
+        FlagGOTO(0);
+    }
+
+    if(cptGlobal>=100) { // Le robot doit s'arrêter à 100sec
+        fnc=0;
+    }
+}

--- a/motors.cpp	Sun Oct 25 22:36:51 2020 +0000
+++ b/motors.cpp	Tue Oct 27 17:27:33 2020 +0000
@@ -1,60 +1,33 @@
-// Nom du fichier : motors.cpp
+/* #include */
 #include "pins.h"
 
+/* Initialisation des drivers */
 void drv_init()
 {
     mot_dis();
-    motGauche_bck();
+    motGauche_fwd();
     motDroite_fwd();
 
-    //motGauche_fwd();
-    //motDroite_bck();
-    //drvGauche.moveLinSpeed(0.01);
-    //drvDroite.moveLinSpeed(0.01);
-    //mode = 0b111; // M0, M1 et M2 sont à 1
-
     mode_M0 = 1;
     //mode_M1 = 1;
     //mode_M2 = 1;
-
-    //mot_en();
-}
-
-// ENABLE/DISABLE // Les deux modules ont le même enable
-
-void mot_en()
-{
-    motG_en();
-    motD_en();
 }
 
-void mot_dis()
-{
-    motG_dis();
-    motD_dis();
-}
-
-void motG_en()
+/* Activation des moteurs */
+void mot_en() // Activation des moteurs
 {
     drvGauche.setEnable(START);
-}
-
-void motD_en()
-{
     drvDroite.setEnable(START);
 }
 
-void motG_dis()
+/* Désactivation des moteurs */
+void mot_dis() // Désactivation des moteurs
 {
     drvGauche.setEnable(STOP);
-}
-
-void motD_dis()
-{
     drvDroite.setEnable(STOP);
 }
 
-// FORWARD
+/* Rotation moteur vers l'avant */
 void motGauche_fwd()
 {
     drvGauche.setDir(FORWARD);
@@ -67,7 +40,7 @@
     //drvDroite.setDir(FORWARD);
 }
 
-// BACKWARD
+/* Rotation moteur vers l'arrière */
 void motGauche_bck()
 {
     drvGauche.setDir(BACKWARD);
@@ -80,6 +53,7 @@
     //drvDroite.setDir(BACKWARD);
 }
 
+/* Changement de vitesse des moteurs */
 void vitesseMotG(double vitesse)
 {
     drvGauche.moveLinSpeed((double)vitesse);
@@ -90,65 +64,8 @@
     drvDroite.moveLinSpeed((double)vitesse);
 }
 
-
-// FONCTIONS TESTS
-//
+/* Fonction test des moteurs */
 void test_drv()
 {
-    /*
-    mot_en();
-    motGauche_fwd();
-    motDroite_fwd();
-    drvGauche.moveLinSpeed(0.250); // 0.035
-    drvDroite.moveLinSpeed(0.250); // 0.035
-    wait(2);
-    motGauche_bck();
-    motDroite_bck();
-    wait(2);
-    mot_dis();
-    */
-
-    /*
-    mot_en();
-    motGauche_fwd();
-    motDroite_fwd();
-    wait(2);
-    mot_dis();
-    */
-
-    mot_en();
-    mot_acc();
-    wait(2);
-    mot_dec();
-    mot_dis();
-
-    drvDroite.moveLinSpeed(0.050);
+    /* Rien */
 }
-
-void mot_acc()
-{
-    double i = 0;
-
-    while (i < vitesseSAT) {
-        bt.printf("mot_acc() => i = %lf\r\n",i);
-        drvDroite.moveLinSpeed(i);
-        drvGauche.moveLinSpeed(i);
-        i+=0.005;
-        wait_ms(10);
-    }
-}
-
-void mot_dec()
-{
-    double i = vitesseSAT;
-
-    while (i > 0) {
-        bt.printf("mot_dec() => i = %lf\r\n",i);
-        drvDroite.moveLinSpeed(i);
-        drvGauche.moveLinSpeed(i);
-        i-=0.005;
-        wait_ms(10);
-    }
-}
-
-void testAngle(int cmdAngle) {}

--- a/motors.h	Sun Oct 25 22:36:51 2020 +0000
+++ b/motors.h	Tue Oct 27 17:27:33 2020 +0000
@@ -5,10 +5,10 @@
 
 void mot_en();
 void mot_dis();
-void motG_en();
-void motD_en();
-void motG_dis();
-void motD_dis();
+//void motG_en();
+//void motD_en();
+//void motG_dis();
+//void motD_dis();
 void motGauche_fwd();
 void motDroite_fwd();
 void motGauche_bck();
@@ -17,8 +17,3 @@
 void vitesseMotD(double vitesse);
 
 void test_drv();
-void testAngle(int cmdAngle);
-//double mot_acc(double vit,double distanceParcourue, double distAfaire);
-//double mot_dec(double vit,double distanceParcourue, double distAfaire);
-void mot_acc();
-void mot_dec();

--- a/odo_asserv.cpp	Sun Oct 25 22:36:51 2020 +0000
+++ b/odo_asserv.cpp	Tue Oct 27 17:27:33 2020 +0000
@@ -1,33 +1,31 @@
-//Nom du fichier : odo_asserv.cpp
+/* #include */
 #include "pins.h"
 
-#define VMAXROT 0.050 // 0.030
-#define VMAXLIN 0.130 // 0.050
-
-//const float DISTLIM = 150.0;
+/* #define & constantes */
+#define VMAXROT 0.050
+#define VMAXLIN 0.100
+#define entraxe 253 // (Valeur théorique = 255)
+const double coeffGLong = 5.956, coeffDLong = 5.956; // tics/millimètre
 
-int cptErreur = 0;
-
-// Objectifs
-//#define NbObj 5
+/* Stratégie */
 int indice = 0;
-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};
+/*
+int objEtape[NbObj] = {0,1,1,1,1,1,1,1,1,1,1,1,1}; // Stratégie côté bleu
+double objX[NbObj] =  {110,645,645,468,645,336,189,200,645,468,645,371,215};
+double objY[NbObj] =  {1085,1200,1490,1490,1490,1788,1032,1200,920,920,920,519,780};
+int objRecule[NbObj]= {0,0,0,0,1,0,0,1,0,0,1,0,0};
+*/
 
-///// VARIABLES
-Ticker ticker_odo;
-Ticker ticker_asserv;
+int objEtape[NbObj] = {0,1,1,1,1,1,1,1,1,1,1,1,1}; // Stratégie côté bleu
+double objX[NbObj] =  {110,645,645,200,645,336,189,200,645,468,645,371,215};
+double objY[NbObj] =  {1085,1200,1400,1400,1400,1788,1032,1200,920,920,920,519,780};
+int objRecule[NbObj]= {0,0,0,0,1,0,0,1,0,0,1,0,0};
 
-// 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
+/* Variable globale */
+Ticker Ticker_asserv;
 
 long comptG = 0, comptD = 0; // nb de tics comptés pour chaque codeur
 
-//bool objOnce = 0;
-
 ///// INTERRUPTIONS CODEURS
 
 void cdgaRise()
@@ -42,13 +40,7 @@
     else comptD++;
 }
 
-///*
-// odo2()
-//#define diametre 51.45 // 51.45 théorique
-//#define N 1000 // 1000 théorique
-#define entraxe 253 // 255 théorique
-//const double coeffG = ((double)(diametre/2)/(double)N)*2.0f*3.1415f;
-//const double coeffD = ((double)(diametre/2)/(double)N)*2.0f*3.1415f;
+
 const double coeffG = 0.16008537;
 const double coeffD = 0.16059957;
 double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
@@ -75,7 +67,6 @@
 double distanceCible = 0;
 double xC = 0, yC = 0; // x = xR et y = yR
 double consigneOrientation = 0;
-//double consigneOrientation = (90*3.1415)/180;
 int signe = 1;
 double cmdD = 0, cmdG = 0;
 double erreurAngle = 0;
@@ -85,7 +76,6 @@
 const double coeffDer = 0.060; // 0.023 de base
 
 // Ligne droite
-//double erreurDist = 0;
 double erreurPreDist = 0;
 double deltaErreurDist = 0;
 const double coeffProDist = 0.0005; // 0.010 de base
@@ -97,8 +87,12 @@
 double distancePrecedente = 0;
 bool stt = 0; // Dépassement du point
 
-// Reculer
-double distanceFaite = 0;
+double OrientationMem = 0;
+double OrPlusPi2 = 0, OrMoinsPi2 = 0;
+
+// Controle dépassement cible
+double distanceMem = 0;
+double distancePlus = 0;
 
 void asserv()
 {
@@ -151,10 +145,6 @@
 
             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();
@@ -162,52 +152,6 @@
                 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;
@@ -219,7 +163,7 @@
             double deltaCommande = (abs(coeffPro * erreurAngle) + abs(coeffDer * deltaErreur));
 
             if(acc) {
-                cmdG = cmdG + 0.0007; // +0.0008
+                cmdG = cmdG + 0.0005; // +0.0008
                 cmdD = cmdG;
 
                 if (cmdG >= VMAXROT) acc = 0;
@@ -239,18 +183,19 @@
             vitesseMotD(abs(cmdD));
 
             if (O > (consigneOrientation - (1*0.0174533)) && O < (consigneOrientation + (1*0.0174533))) {
-                //mot_dis();
                 fnc++;
                 rebooted = 1;
                 acc = 1;
-                //objOnce = 0;
+                //azerty();
+                distanceMem = distanceCible;
+                distancePlus = 0;
                 break;
             }
 
             break;
 
         case 2: // Avancer
-        
+
             if (rebooted == 1 && wtt==1) {
                 cmdG = 0;
                 cmdD = 0;
@@ -259,53 +204,6 @@
                 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
-            if(cmdD>VMAXLIN) {
-                cmdD = VMAXLIN;
-            }
-            cmdG = cmdD;
-            */
-
-            // Asservissement distance à parcourir
-            //erreurDist =  consigneOrientation - O;
-
             deltaErreurDist = distanceCible - erreurPreDist;
 
             erreurPreDist  = distanceCible;
@@ -313,15 +211,13 @@
             double deltaCommande2 = (abs(coeffProDist * distanceCible) + abs(coeffDerDist * deltaErreurDist));
 
             if(acc) {
-                cmdG = cmdG + 0.0012; // +0.0008
+                cmdG = cmdG + 0.001; // +0.0008
                 cmdD = cmdG;
 
                 if (cmdG >= VMAXLIN) {
                     acc = 0;
-                    //stt = 1;
                 }
             } else {
-                //acc = 0;
 
                 if (deltaCommande2 < VMAXLIN) {
                     cmdG = deltaCommande2;
@@ -332,24 +228,11 @@
                 }
             }
 
-            //vitesseMotG(abs(cmdG));
-            //vitesseMotD(abs(cmdD));
+            distancePlus += abs(dDist);
 
-            //if (distanceCible < 15)[
-            //if ((x < xC+10) && (x > xC-10) && (y < yC+10) && (y > yC-10)) {
-            if ((distanceCible < 10) || (acc==0 && wtt==1 && rebooted==0 && (distancePrecedente < distanceCible))) {
-                //mot_dis();
-                //fnc++;
+            if ((distanceCible < 10) || (distancePlus >= distanceMem)) {
                 acc = 1;
-                //stt = 0;
-
-                //if (objOnce == 0) {
                 indice++;
-                //objOnce=1;
-
-
-
-                //}
 
                 if (indice>=(int)NbObj) {
                     fnc = 0;
@@ -362,15 +245,6 @@
                 distancePrecedente = 0;
                 break;
 
-                /*
-                if (indice>=NbObj) {
-                    fnc = 0;
-                } else {
-                    fnc = objEtape[indice];
-                    xC = objX[indice];
-                    yC = objY[indice];
-                }
-                */
             }
 
             if (objRecule[indice] == 0) {
@@ -387,28 +261,6 @@
             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	Sun Oct 25 22:36:51 2020 +0000
+++ b/odo_asserv.h	Tue Oct 27 17:27:33 2020 +0000
@@ -1,6 +1,6 @@
 // Nom du fichier : odo_asserv.h
 
-#define NbObj 8
+#define NbObj 13
 
 extern int indice;
 extern int objEtape[NbObj];
@@ -25,13 +25,15 @@
 extern double cmdD;
 extern int fnc;
 
+extern double distanceMem;
+extern double distancePlus;
+
 // Prototypes
 void cdgaRise();
 void cddaRise();
 
 //ODOMETRIE
-extern Ticker ticker_odo;
-extern Ticker ticker_asserv;
+extern Ticker Ticker_asserv;
 void odo1();
 void odo2();
 void odo3();

--- a/pins.cpp	Sun Oct 25 22:36:51 2020 +0000
+++ b/pins.cpp	Tue Oct 27 17:27:33 2020 +0000
@@ -1,11 +1,11 @@
-// Nom du fichier : pins.cpp
+/* #include */
 #include "pins.h"
 
-InterruptIn btn(PC_13);
-//DigitalOut myled(LED2);
-//DigitalIn cordon(PC_8);
+DigitalOut Led(LED1); // LED allumée pendant le stand-by
+DigitalIn Tirette(PC_8);
+InterruptIn BtnBlue(PC_13);
 
-// Capteurs à ultrasons
+/* Capteurs à ultrasons */
 DigitalOut trigger(PB_9);
 InterruptIn echo1(PA_11);
 InterruptIn echo2(PB_12);
@@ -14,7 +14,7 @@
 InterruptIn echo5(PB_15);
 InterruptIn echo6(PB_14);
 
-// Drivers DRV8825 
+/* Drivers DRV8825 */
 #define STEP1 PA_6
 #define STEP2 PB_6
 #define DIR1 PC_2
@@ -23,15 +23,14 @@
 #define EN2 PA_14
 #define diametreRoue 51.45
 #define rayonRoue (diametreRoue/2)
-#define nbPas 6400 //1000
-//BusOut mode(PB_7, PC_13, PC_14); // LSB ... MSB
+#define nbPas 6400
 DigitalOut mode_M0(PB_7);
 //DigitalOut mode_M1(PC_13); // Inutile
 //DigitalOut mode_M2(PC_14); // Hardware
-DRV8825 drvGauche(EN1,DIR1,STEP1,rayonRoue,nbPas); 
-DRV8825 drvDroite(EN2,DIR2,STEP2,rayonRoue,nbPas); 
+DRV8825 drvGauche(EN1,DIR1,STEP1,rayonRoue,nbPas);
+DRV8825 drvDroite(EN2,DIR2,STEP2,rayonRoue,nbPas);
 
-// Codeurs (Réf : LPJ3806-1000BM-G5-24E)
+/* Codeurs (Réf : LPJ3806-1000BM-G5-24E) */
 InterruptIn cdgA(PA_8); // Codeur de gauche
 DigitalIn cdgB(PA_9);
 InterruptIn cddA(PA_0); // Codeur de droite

--- a/pins.h	Sun Oct 25 22:36:51 2020 +0000
+++ b/pins.h	Tue Oct 27 17:27:33 2020 +0000
@@ -1,4 +1,4 @@
-// Nom du fichier : pins.h
+/* #include */
 #include "mbed.h"
 #include "DRV8825.h"
 
@@ -10,21 +10,14 @@
 #include "odo_asserv.h"
 #include "Flag.h"
 
-// #define
-//#define Pi 3.14159265359
-#define _PI_ 3.14159265359
-//#define NbPulseCodeur 1000
-//#define ecartCodeuse 245 // Distance en mm entre les deux roues codeuses
-#define diametreRoueCodeuse 51.450 // Diamètre de la roue codeuse en mm
-#define perimetreRoueCodeuse (diametreRoueCodeuse * Pi)
-#define vitesseSAT 0.250 // m/s 
-#define perimetreE Pi*ecartCodeuse
+/* #define */
+//#define _PI_ 3.14159265359
 
-extern InterruptIn btn;
-//extern DigitalOut myled;
-//extern DigitalIn cordon;
+/* extern */
+extern DigitalOut Led;
+extern DigitalIn Tirette;
+extern InterruptIn BtnBlue;
 
-//Capteurs à ultrasons
 extern DigitalOut trigger;
 extern InterruptIn echo1;
 extern InterruptIn echo2;
@@ -33,15 +26,12 @@
 extern InterruptIn echo5;
 extern InterruptIn echo6;
 
-// Drivers DRV8825 
-//extern BusOut mode;
 extern DigitalOut mode_M0;
 //extern DigitalOut mode_M1;
 //extern DigitalOut mode_M2;
 extern DRV8825 drvGauche; 
 extern DRV8825 drvDroite; 
 
-// Codeurs
 extern InterruptIn cdgA;
 extern DigitalIn cdgB;
 extern InterruptIn cddA;