AresENSEA-CDF2020
Test
Diff: odo_asserv.cpp
- Revision:
- 20:7d206773f39e
- Parent:
- 19:c419033c0967
- Child:
- 22:f891c2bce091
--- a/odo_asserv.cpp Mon Oct 12 19:17:40 2020 +0000
+++ b/odo_asserv.cpp Tue Oct 20 17:53:32 2020 +0000
@@ -1,15 +1,20 @@
//Nom du fichier : odo_asserv.cpp
#include "pins.h"
-#define VMAXROT 0.060 // 0.030
-#define VMAXLIN 0.100 // 0.050
+#define VMAXROT 0.050 // 0.030
+#define VMAXLIN 0.130 // 0.050
+
+//const float DISTLIM = 150.0;
+
+int cptErreur = 0;
// Objectifs
-#define NbObj 4
+//#define NbObj 5
int indice = 0;
-int objEtape [4] = {0,1,1,1};
-double objX[4] = {0,660, 660, 210};
-double objY[4] = {0,1070,1650,1300};
+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};
///// VARIABLES
Ticker ticker_odo;
@@ -17,10 +22,11 @@
// 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
+//const double coeffGAngl = 737.447, coeffDAngl = 748.057; // constantes permettant la transformation tic/radian
long comptG = 0, comptD = 0; // nb de tics comptés pour chaque codeur
+//bool objOnce = 0;
///// INTERRUPTIONS CODEURS
@@ -36,8 +42,6 @@
else comptD++;
}
-
-
///*
// odo2()
//#define diametre 51.45 // 51.45 théorique
@@ -59,11 +63,9 @@
y += (double) dDist * sin(O);
O += (double) dAngl;
-
if (O > 3.1415) O = O - (2.0f * 3.1415f);
if (O < -3.1415) O = O + (2.0f * 3.1415f);
-
comptG = 0;
comptD = 0;
}
@@ -79,21 +81,24 @@
double erreurAngle = 0;
double erreurPre = 0;
double deltaErreur = 0;
-const double coeffPro = 0.08; // 0.023 de base
-const double coeffDer = 0.06; // 0.023 de base
+const double coeffPro = 0.075; // 0.023 de base
+const double coeffDer = 0.060; // 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
+const double coeffProDist = 0.0005; // 0.010 de base
+const double coeffDerDist = 0.0005; // 0.010 de base
// NEW NEW NEW NEW
int fnc = 0;
bool acc = 1;
double distancePrecedente = 0;
-bool stt = 0;
+bool stt = 0; // Dépassement du point
+
+// Reculer
+double distanceFaite = 0;
void asserv()
{
@@ -126,19 +131,83 @@
case 1: // Rotation
mot_en();
+ // Si on doit reculer
+ if (objRecule[indice]==1) {
+ consigneOrientation += 3.1415;
+
+ if(consigneOrientation>3.1415) consigneOrientation-=2*3.1415;
+ if(consigneOrientation<-3.1415) consigneOrientation+=2*3.1415;
+ }
+
// 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);
+ double Osens = 0;
+ if (O<0) Osens = O + (2.0f*3.1415f);
+ else Osens = O;
+
+ double consigneSens = 0;
+ if (consigneOrientation<0) consigneSens = consigneOrientation + (2.0f*3.1415f);
+ else consigneSens = consigneOrientation;
- if (choixSens > 0) {
+ 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();
} 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();
+ }
+ }
+ */
+
+ /*
+ 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;
@@ -150,7 +219,7 @@
double deltaCommande = (abs(coeffPro * erreurAngle) + abs(coeffDer * deltaErreur));
if(acc) {
- cmdG = cmdG + 0.0005;
+ cmdG = cmdG + 0.0007; // +0.0008
cmdD = cmdG;
if (cmdG >= VMAXROT) acc = 0;
@@ -172,12 +241,59 @@
if (O > (consigneOrientation - (1*0.0174533)) && O < (consigneOrientation + (1*0.0174533))) {
//mot_dis();
fnc++;
+ rebooted = 1;
acc = 1;
+ //objOnce = 0;
+ break;
}
+
break;
case 2: // Avancer
- mot_en();
+
+ if (rebooted == 1 && wtt==1) {
+ cmdG = 0;
+ cmdD = 0;
+ rebooted = 0;
+ acc=1;
+ 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
@@ -197,12 +313,12 @@
double deltaCommande2 = (abs(coeffProDist * distanceCible) + abs(coeffDerDist * deltaErreurDist));
if(acc) {
- cmdG = cmdG + 0.0005;
+ cmdG = cmdG + 0.0012; // +0.0008
cmdD = cmdG;
if (cmdG >= VMAXLIN) {
acc = 0;
- stt = 1;
+ //stt = 1;
}
} else {
//acc = 0;
@@ -216,33 +332,83 @@
}
}
- vitesseMotG(abs(cmdG));
- vitesseMotD(abs(cmdD));
+ //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))) {
+ if ((distanceCible < 10) || (acc==0 && wtt==1 && rebooted==0 && (distancePrecedente < distanceCible))) {
//mot_dis();
- //fnc=0;
+ //fnc++;
acc = 1;
- stt = 0;
+ //stt = 0;
+
+ //if (objOnce == 0) {
indice++;
- fnc = objEtape[indice];
- xC = objX[indice];
- yC = objY[indice];
+ //objOnce=1;
+
+
+
+ //}
+
+ if (indice>=(int)NbObj) {
+ fnc = 0;
+ } else {
+ fnc = objEtape[indice];
+ xC = objX[indice];
+ yC = objY[indice];
+ }
+
+ distancePrecedente = 0;
+ break;
+
+ /*
+ if (indice>=NbObj) {
+ fnc = 0;
+ } else {
+ fnc = objEtape[indice];
+ xC = objX[indice];
+ yC = objY[indice];
+ }
+ */
}
- motGauche_fwd();
- motDroite_fwd();
+ if (objRecule[indice] == 0) {
+ motGauche_fwd();
+ motDroite_fwd();
+ } else {
+ motGauche_bck();
+ motDroite_bck();
+ }
+
vitesseMotG(cmdG);
vitesseMotD(cmdD);
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();