AresENSEA-CDF2020
/
AresCDFMainCode_us2
Test
Diff: odo_asserv.cpp
- Revision:
- 19:c419033c0967
- Parent:
- 18:48246daf0c06
- Child:
- 20:7d206773f39e
diff -r 48246daf0c06 -r c419033c0967 odo_asserv.cpp --- a/odo_asserv.cpp Wed Oct 07 20:00:45 2020 +0000 +++ b/odo_asserv.cpp Mon Oct 12 19:17:40 2020 +0000 @@ -1,7 +1,15 @@ //Nom du fichier : odo_asserv.cpp #include "pins.h" -#define VMAX 0.020 +#define VMAXROT 0.060 // 0.030 +#define VMAXLIN 0.100 // 0.050 + +// Objectifs +#define NbObj 4 +int indice = 0; +int objEtape [4] = {0,1,1,1}; +double objX[4] = {0,660, 660, 210}; +double objY[4] = {0,1070,1650,1300}; ///// VARIABLES Ticker ticker_odo; @@ -28,26 +36,8 @@ else comptD++; } -/* -// odo1() -double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation -double x = 0, y = 0, O = 0; -void odo1() -{ - dDist = ((comptG / coeffGLong) + (comptD / coeffDLong)) / 2; - dAngl = ((comptD / coeffDAngl) - (comptG / coeffGAngl)); - - x += dDist * cos(dAngl); - y += dDist * sin(dAngl); - O += dAngl; - - comptG = 0; - comptD = 0; -} -*/ - ///* // odo2() //#define diametre 51.45 // 51.45 théorique @@ -58,7 +48,7 @@ const double coeffG = 0.16008537; const double coeffD = 0.16059957; double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation -double x = 0, y = 0, O = 0; +double x = 110, y = 1070, O = 0; void odo2() { @@ -79,28 +69,6 @@ } //*/ -/* -// odo3() -#define diametre 51.45 -#define N 1000 -#define entraxe 255 -const double coeffG = 1/5.956; -const double coeffD = 1/5.956; - -void odo3() -{ - dDist = (double) ((comptG * coeffG) + (comptD * coeffD)) / 2.0f; - dAngl = (double) ((comptD * coeffD) - (comptG * coeffG)) / entraxe; - - x += (double) dDist * cos(O); - y += (double) dDist * sin(O); - O += (double) dAngl; - - comptG = 0; - comptD = 0; -} -*/ - double distanceCible = 0; double xC = 0, yC = 0; // x = xR et y = yR @@ -111,12 +79,21 @@ double erreurAngle = 0; double erreurPre = 0; double deltaErreur = 0; -const double coeffPro = 0.06; // 0.023 de base +const double coeffPro = 0.08; // 0.023 de base const double coeffDer = 0.06; // 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 + // NEW NEW NEW NEW int fnc = 0; bool acc = 1; +double distancePrecedente = 0; +bool stt = 0; void asserv() { @@ -149,7 +126,13 @@ case 1: // Rotation mot_en(); - if (consigneOrientation > 0) { + // 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); + + if (choixSens > 0) { motGauche_bck(); motDroite_fwd(); } else { @@ -170,15 +153,15 @@ cmdG = cmdG + 0.0005; cmdD = cmdG; - if (cmdG >= VMAX) acc = 0; + if (cmdG >= VMAXROT) acc = 0; } else { //acc = 0; - if (deltaCommande < VMAX) { + if (deltaCommande < VMAXROT) { cmdG = deltaCommande; cmdD = cmdG; } else { - cmdG = VMAX; + cmdG = VMAXROT; cmdD = cmdG; } } @@ -186,7 +169,7 @@ vitesseMotG(abs(cmdG)); vitesseMotD(abs(cmdD)); - if (O > (consigneOrientation - (2*0.0174533)) && O < (consigneOrientation + (2*0.0174533))) { + if (O > (consigneOrientation - (1*0.0174533)) && O < (consigneOrientation + (1*0.0174533))) { //mot_dis(); fnc++; acc = 1; @@ -196,21 +179,66 @@ case 2: // Avancer mot_en(); - cmdD = abs((int)distanceCible)*0.0005; - if(cmdD>VMAX) { - cmdD = VMAX; + /* + 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; + + double deltaCommande2 = (abs(coeffProDist * distanceCible) + abs(coeffDerDist * deltaErreurDist)); + + if(acc) { + cmdG = cmdG + 0.0005; + cmdD = cmdG; + + if (cmdG >= VMAXLIN) { + acc = 0; + stt = 1; + } + } else { + //acc = 0; + + if (deltaCommande2 < VMAXLIN) { + cmdG = deltaCommande2; + cmdD = cmdG; + } else { + cmdG = VMAXLIN; + cmdD = cmdG; + } + } + + 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))) { + //mot_dis(); + //fnc=0; + acc = 1; + stt = 0; + indice++; + fnc = objEtape[indice]; + xC = objX[indice]; + yC = objY[indice]; + } motGauche_fwd(); motDroite_fwd(); vitesseMotG(cmdG); vitesseMotD(cmdD); - if (abs((int)distanceCible) < 3) { - //mot_dis(); - fnc=0; - } + distancePrecedente = distanceCible; + break; case 3: // Reculer