Diff: odo_asserv.cpp

Revision:

25:869b1c1f51a7

Parent:

24:be2b2be6907b

Child:

26:bb2b778bd351

--- a/odo_asserv.cpp	Wed Oct 28 20:08:39 2020 +0000
+++ b/odo_asserv.cpp	Thu Oct 29 07:53:25 2020 +0000
@@ -2,41 +2,70 @@
 #include "pins.h"
 
 /* #define & constantes */
-#define VMAXROT 0.020
-#define VMAXLIN 0.040
-#define entraxe 253 // (Valeur théorique = 255)
-const double coeffGLong = 5.956, coeffDLong = 5.956; // tics/millimètre
+#define VMAXROT 0.040 // 0.020
+#define VMAXLIN 0.100 // 0.060
+
+/* Choix côté */
 
-/* Stratégie */
-int indice = 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};
-*/
+// Côté bleu
+#define X_INIT 110
+#define Y_INIT 1070
+#define O_INIT 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,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};
+// Côté jaune
+#define X_INIT 2890
+#define Y_INIT 1070
+#define O_INIT 3.1414
 */
 
 
-//int objEtape[NbObj] = {0,1,1,1}; // Stratégie côté bleu
-int objEtape [4] = {0,1,1,1};
-double objX[4] = {0,660, 660, 210};
-double objY[4] = {0,1070,1650,1300};
-int objRecule[NbObj]= {0,0,0,0};
+#define entraxe 253 // (Valeur théorique = 255)
+const double coeffG = 0.16008537; // mm/tic
+const double coeffD = 0.16059957; // mm/tic
+/* COEFF ROTATION */
+const double coeffPro = 0.040; // 0.020
+const double coeffDer = 0.030; // 0.010
+/* COEFF LIGNE DROITE */
+const double coeffProDist = 0.003; // 0.0008
+const double coeffDerDist = 0.003; // 0.0008
+
+#define ACC_ROT 0.002
+#define ACC_LIN 0.002
+
+/* Stratégie */
+int indiceStrategie = 0;
+
+/* // Stratégie ultra simple
+int objEtape[NbObj] = {0,1,1,1};
+double objX[NbObj] = {0,660, 660, 210};
+double objY[NbObj] = {0,1070,1650,1300};
+int objRecule[NbObj]= {0,0,0,0};*/
 
 
-/* Variable globale */
-Ticker Ticker_asserv;
+ // Stratégie Lucas Blue
+int objEtape[NbObj] = {0,1,1,1,1,1,1,1,1,1};
+double objX[NbObj] = {110,200,660,627,450,235,698,300,300,230};
+double objY[NbObj] = {1070,1070,1200,743,920,1124,672,460,1100,750};
+int objRecule[NbObj]= {0,0,0,0,0,0,1,0,0,1}; 
 
-long comptG = 0, comptD = 0; // nb de tics comptés pour chaque codeur
+/*// Stratégie Lucas Yellow
+int objEtape[NbObj] = {0,1,1,1,1,1,1,1,1,1};
+double objX[NbObj] = {2890,2800,2340,2373,2550,2765,2302,2700,2700,2800};
+double objY[NbObj] = {1070,1070,1200,743,920,1124,672,460,1100,750};
+int objRecule[NbObj]= {0,0,0,0,0,0,1,0,0,1};
+*/
 
-///// INTERRUPTIONS CODEURS
+/* Variable globale */
+Ticker Ticker_asserv; // Ticker pour l'asservissement en position
+
+int action = 0; // Actions possibles : Rotation & Avancer/Reculer
+long comptG = 0, comptD = 0; // Nb de tics compté par roue
+double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
+double x = X_INIT, y = Y_INIT, O = O_INIT;
+
+/* Interruptions codeurs */
 
 void cdgaRise()
 {
@@ -51,10 +80,7 @@
 }
 
 
-const double coeffG = 0.16008537;
-const double coeffD = 0.16059957;
-double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
-double x = 110, y = 1070, O = 0;
+
 
 void odo2()
 {
@@ -82,17 +108,15 @@
 double erreurAngle = 0;
 double erreurPre = 0;
 double deltaErreur = 0;
-const double coeffPro = 0.020; // 0.023 de base
-const double coeffDer = 0.010; // 0.023 de base
+
 
 // Ligne droite
 double erreurPreDist = 0;
 double deltaErreurDist = 0;
-const double coeffProDist = 0.0008; // 0.010 de base
-const double coeffDerDist = 0.0008; // 0.010 de base
+
 
 // NEW NEW NEW NEW
-int fnc = 0;
+
 bool acc = 1;
 double distancePrecedente = 0;
 bool stt = 0; // Dépassement du point
@@ -127,7 +151,7 @@
 
     // Switch de sélection de l'étape
 
-    switch (fnc) {
+    switch (action) {
         case 0: // Stand-by
             mot_dis();
             break;
@@ -136,7 +160,7 @@
             mot_en();
 
             // Si on doit reculer
-            if (objRecule[indice]==1) {
+            if (objRecule[indiceStrategie]==1) {
                 consigneOrientation += 3.1415;
 
                 if(consigneOrientation>3.1415) consigneOrientation-=2*3.1415;
@@ -173,7 +197,7 @@
             double deltaCommande = (abs(coeffPro * erreurAngle) + abs(coeffDer * deltaErreur));
 
             if(acc) {
-                cmdG = cmdG + 0.0008; // +0.0008
+                cmdG = cmdG + ACC_ROT; // +0.0008
                 cmdD = cmdG;
 
                 if (cmdG >= VMAXROT) acc = 0;
@@ -193,8 +217,8 @@
             vitesseMotD(abs(cmdD));
 
             if (O > (consigneOrientation - (1*0.0174533)) && O < (consigneOrientation + (1*0.0174533))) {
-                fnc++;
-                rebooted = 1;
+                action++;
+                us_rbt_restart = 1;
                 acc = 1;
                 //azerty();
                 distanceMem = distanceCible;
@@ -206,10 +230,10 @@
 
         case 2: // Avancer
 
-            if (rebooted == 1 && wtt==1) {
+            if (us_rbt_restart == 1 && us_libre == 1) {
                 cmdG = 0;
                 cmdD = 0;
-                rebooted = 0;
+                us_rbt_restart = 0;
                 acc=1;
                 mot_en();
             }
@@ -221,7 +245,7 @@
             double deltaCommande2 = (abs(coeffProDist * distanceCible) + abs(coeffDerDist * deltaErreurDist));
 
             if(acc) {
-                cmdG = cmdG + 0.0006; // +0.0008
+                cmdG = cmdG + ACC_LIN; // +0.0006
                 cmdD = cmdG;
 
                 if (cmdG >= VMAXLIN) {
@@ -242,22 +266,21 @@
 
             if ((distanceCible < 10) || (distancePlus >= distanceMem)) {
                 acc = 1;
-                indice++;
+                indiceStrategie++;
 
-                if (indice>=(int)NbObj) {
-                    fnc = 0;
+                if (indiceStrategie >= (int)NbObj) {
+                    action = 0;
                 } else {
-                    fnc = objEtape[indice];
-                    xC = objX[indice];
-                    yC = objY[indice];
+                    action = objEtape[indiceStrategie];
+                    xC = objX[indiceStrategie];
+                    yC = objY[indiceStrategie];
                 }
 
                 distancePrecedente = 0;
                 break;
-
             }
 
-            if (objRecule[indice] == 0) {
+            if (objRecule[indiceStrategie] == 0) {
                 motGauche_fwd();
                 motDroite_fwd();
             } else {