Diff: odo_asserv.cpp

Revision:

22:f891c2bce091

Parent:

20:7d206773f39e

Child:

23:a74135a0271d

--- 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();