Revision 19:c419033c0967, committed 2020-10-12

Comitter:

Nanaud

Date:

Mon Oct 12 19:17:40 2020 +0000

Parent:

18:48246daf0c06

Child:

20:7d206773f39e

Commit message:

Dernieres modifs

Changed in this revision

--- a/debugPC.cpp	Wed Oct 07 20:00:45 2020 +0000
+++ b/debugPC.cpp	Mon Oct 12 19:17:40 2020 +0000
@@ -186,10 +186,15 @@
             //comptD = 0;
             //test1();
             //consigneOrientation = (180*3.1415)/180;
-            xC = (double)0;
-            yC = (double)100;
-            fnc=1;
-            //mot_en();
+            //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");
@@ -294,6 +299,7 @@
             bt.printf("O = %lf\n\r", O);
             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);

--- a/main.cpp	Wed Oct 07 20:00:45 2020 +0000
+++ b/main.cpp	Mon Oct 12 19:17:40 2020 +0000
@@ -13,12 +13,24 @@
     bt.printf("comptD = %d\r\n",comptD);
 }
 
+/*
+void cordonDem()
+{
+    indice++;
+    fnc = objEtape[indice];
+    xC = objX[indice];
+    yC = objY[indice];
+    mot_en();
+}
+*/
+
 int main()
 {
     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
@@ -32,7 +44,7 @@
     //ticker_affUS.attach(&affUltrasons,1.0);
     //ticker_affcd.attach(&affCodeurs,1.0);
     //ticker_odo.attach(&odo2,0.02);
-    ticker_asserv.attach(&asserv,0.020);
+    ticker_asserv.attach(&asserv,0.015);
     //ticker_affodo.attach(&affOdo,1.0);
 
     // Init capteurs à ultrasons
@@ -60,6 +72,10 @@
     cddA.mode(PullUp);
 
     while(1) {
-        //odoGeonobot1();
+        if (indice>=4) {
+            fnc = 0;
+            mot_dis();
+        }
+
     }
 }

--- a/odo_asserv.cpp	Wed Oct 07 20:00:45 2020 +0000
+++ b/odo_asserv.cpp	Mon Oct 12 19:17:40 2020 +0000
@@ -1,7 +1,15 @@
 //Nom du fichier : odo_asserv.cpp
 #include "pins.h"
 
-#define VMAX 0.020
+#define VMAXROT 0.060 // 0.030
+#define VMAXLIN 0.100 // 0.050
+
+// Objectifs
+#define NbObj 4
+int indice = 0;
+int objEtape [4] = {0,1,1,1};
+double objX[4] = {0,660, 660, 210};
+double objY[4] = {0,1070,1650,1300};
 
 ///// VARIABLES
 Ticker ticker_odo;
@@ -28,26 +36,8 @@
     else comptD++;
 }
 
-/*
-// odo1()
-double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
-double x = 0, y = 0, O = 0;
 
 
-void odo1()
-{
-    dDist = ((comptG / coeffGLong) + (comptD / coeffDLong)) / 2;
-    dAngl = ((comptD / coeffDAngl) - (comptG / coeffGAngl));
-
-    x += dDist * cos(dAngl);
-    y += dDist * sin(dAngl);
-    O += dAngl;
-
-    comptG = 0;
-    comptD = 0;
-}
-*/
-
 ///*
 // odo2()
 //#define diametre 51.45 // 51.45 théorique
@@ -58,7 +48,7 @@
 const double coeffG = 0.16008537;
 const double coeffD = 0.16059957;
 double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
-double x = 0, y = 0, O = 0;
+double x = 110, y = 1070, O = 0;
 
 void odo2()
 {
@@ -79,28 +69,6 @@
 }
 //*/
 
-/*
-// odo3()
-#define diametre 51.45
-#define N 1000
-#define entraxe 255
-const double coeffG = 1/5.956;
-const double coeffD = 1/5.956;
-
-void odo3()
-{
-    dDist = (double) ((comptG * coeffG) + (comptD * coeffD)) / 2.0f;
-    dAngl = (double) ((comptD * coeffD) - (comptG * coeffG)) / entraxe;
-
-    x += (double) dDist * cos(O);
-    y += (double) dDist * sin(O);
-    O += (double) dAngl;
-
-    comptG = 0;
-    comptD = 0;
-}
-*/
-
 
 double distanceCible = 0;
 double xC = 0, yC = 0; // x = xR et y = yR
@@ -111,12 +79,21 @@
 double erreurAngle = 0;
 double erreurPre = 0;
 double deltaErreur = 0;
-const double coeffPro = 0.06; // 0.023 de base
+const double coeffPro = 0.08; // 0.023 de base
 const double coeffDer = 0.06; // 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
+
 // NEW NEW NEW NEW
 int fnc = 0;
 bool acc = 1;
+double distancePrecedente = 0;
+bool stt = 0;
 
 void asserv()
 {
@@ -149,7 +126,13 @@
         case 1: // Rotation
             mot_en();
 
-            if (consigneOrientation > 0) {
+            // 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);
+
+            if (choixSens > 0) {
                 motGauche_bck();
                 motDroite_fwd();
             } else {
@@ -170,15 +153,15 @@
                 cmdG = cmdG + 0.0005;
                 cmdD = cmdG;
 
-                if (cmdG >= VMAX) acc = 0;
+                if (cmdG >= VMAXROT) acc = 0;
             } else {
                 //acc = 0;
 
-                if (deltaCommande < VMAX) {
+                if (deltaCommande < VMAXROT) {
                     cmdG = deltaCommande;
                     cmdD = cmdG;
                 } else {
-                    cmdG = VMAX;
+                    cmdG = VMAXROT;
                     cmdD = cmdG;
                 }
             }
@@ -186,7 +169,7 @@
             vitesseMotG(abs(cmdG));
             vitesseMotD(abs(cmdD));
 
-            if (O > (consigneOrientation - (2*0.0174533)) && O < (consigneOrientation + (2*0.0174533))) {
+            if (O > (consigneOrientation - (1*0.0174533)) && O < (consigneOrientation + (1*0.0174533))) {
                 //mot_dis();
                 fnc++;
                 acc = 1;
@@ -196,21 +179,66 @@
         case 2: // Avancer
             mot_en();
 
-            cmdD = abs((int)distanceCible)*0.0005;
-            if(cmdD>VMAX) {
-                cmdD = VMAX;
+            /*
+            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;
+
+            double deltaCommande2 = (abs(coeffProDist * distanceCible) + abs(coeffDerDist * deltaErreurDist));
+
+            if(acc) {
+                cmdG = cmdG + 0.0005;
+                cmdD = cmdG;
+
+                if (cmdG >= VMAXLIN) {
+                    acc = 0;
+                    stt = 1;
+                }
+            } else {
+                //acc = 0;
+
+                if (deltaCommande2 < VMAXLIN) {
+                    cmdG = deltaCommande2;
+                    cmdD = cmdG;
+                } else {
+                    cmdG = VMAXLIN;
+                    cmdD = cmdG;
+                }
+            }
+
+            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))) {
+                //mot_dis();
+                //fnc=0;
+                acc = 1;
+                stt = 0;
+                indice++;
+                fnc = objEtape[indice];
+                xC = objX[indice];
+                yC = objY[indice];
+            }
 
             motGauche_fwd();
             motDroite_fwd();
             vitesseMotG(cmdG);
             vitesseMotD(cmdD);
 
-            if (abs((int)distanceCible) < 3) {
-                //mot_dis();
-                fnc=0;
-            }
+            distancePrecedente = distanceCible;
+
             break;
 
         case 3: // Reculer

--- a/odo_asserv.h	Wed Oct 07 20:00:45 2020 +0000
+++ b/odo_asserv.h	Mon Oct 12 19:17:40 2020 +0000
@@ -1,5 +1,10 @@
 // Nom du fichier : odo_asserv.h
 
+extern int indice;
+extern int objEtape[4];
+extern double objX[4];
+extern double objY[4];
+
 // extern
 // CODEURS
 extern const double coeffGLong;

--- a/pins.cpp	Wed Oct 07 20:00:45 2020 +0000
+++ b/pins.cpp	Mon Oct 12 19:17:40 2020 +0000
@@ -2,6 +2,7 @@
 #include "pins.h"
 
 InterruptIn btn(PC_13);
+//InterruptIn cordon(PC_8);
 
 // Capteurs à ultrasons
 DigitalOut trigger(PB_9);

--- a/pins.h	Wed Oct 07 20:00:45 2020 +0000
+++ b/pins.h	Mon Oct 12 19:17:40 2020 +0000
@@ -20,6 +20,7 @@
 #define perimetreE Pi*ecartCodeuse
 
 extern InterruptIn btn;
+//extern InterruptIn cordon;
 
 //Capteurs à ultrasons
 extern DigitalOut trigger;