AresENSEA-CDF2020
Test
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();