AresENSEA-CDF2020
/
AresCDFMainCode_us2
Test
Diff: odo_asserv.cpp
- Revision:
- 22:f891c2bce091
- Parent:
- 20:7d206773f39e
- Child:
- 23:a74135a0271d
diff -r e5f0f5abb5ae -r f891c2bce091 odo_asserv.cpp --- 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();