Diff: odo_asserv.cpp

Revision:

19:c419033c0967

Parent:

18:48246daf0c06

Child:

20:7d206773f39e

--- 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